drivers/media/i2c/ov5640.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved.
   4  * Copyright (C) 2014-2017 Mentor Graphics Inc.
   5  */
   6
   7 #include <linux/clk.h>
   8 #include <linux/clk-provider.h>
   9 #include <linux/clkdev.h>
  10 #include <linux/ctype.h>
  11 #include <linux/delay.h>
  12 #include <linux/device.h>
  13 #include <linux/gpio/consumer.h>
  14 #include <linux/i2c.h>
  15 #include <linux/init.h>
  16 #include <linux/module.h>
  17 #include <linux/of_device.h>
  18 #include <linux/regulator/consumer.h>
  19 #include <linux/slab.h>
  20 #include <linux/types.h>
  21 #include <media/v4l2-async.h>
  22 #include <media/v4l2-ctrls.h>
  23 #include <media/v4l2-device.h>
  24 #include <media/v4l2-event.h>
  25 #include <media/v4l2-fwnode.h>
  26 #include <media/v4l2-subdev.h>
  27
  28 /* min/typical/max system clock (xclk) frequencies */
  29 #define OV5640_XCLK_MIN  6000000
  30 #define OV5640_XCLK_MAX 54000000
  31
  32 #define OV5640_NATIVE_WIDTH             2624
  33 #define OV5640_NATIVE_HEIGHT            1964
  34 #define OV5640_PIXEL_ARRAY_TOP          14
  35 #define OV5640_PIXEL_ARRAY_LEFT         16
  36 #define OV5640_PIXEL_ARRAY_WIDTH        2592
  37 #define OV5640_PIXEL_ARRAY_HEIGHT       1944
  38
  39 /* FIXME: not documented. */
  40 #define OV5640_MIN_VBLANK       24
  41 #define OV5640_MAX_VTS          3375
  42
  43 #define OV5640_DEFAULT_SLAVE_ID 0x3c
  44
  45 #define OV5640_LINK_RATE_MAX            490000000U
  46
  47 #define OV5640_REG_SYS_RESET02          0x3002
  48 #define OV5640_REG_SYS_CLOCK_ENABLE02   0x3006
  49 #define OV5640_REG_SYS_CTRL0            0x3008
  50 #define OV5640_REG_SYS_CTRL0_SW_PWDN    0x42
  51 #define OV5640_REG_SYS_CTRL0_SW_PWUP    0x02
  52 #define OV5640_REG_CHIP_ID              0x300a
  53 #define OV5640_REG_IO_MIPI_CTRL00       0x300e
  54 #define OV5640_REG_PAD_OUTPUT_ENABLE01  0x3017
  55 #define OV5640_REG_PAD_OUTPUT_ENABLE02  0x3018
  56 #define OV5640_REG_PAD_OUTPUT00         0x3019
  57 #define OV5640_REG_SYSTEM_CONTROL1      0x302e
  58 #define OV5640_REG_SC_PLL_CTRL0         0x3034
  59 #define OV5640_REG_SC_PLL_CTRL1         0x3035
  60 #define OV5640_REG_SC_PLL_CTRL2         0x3036
  61 #define OV5640_REG_SC_PLL_CTRL3         0x3037
  62 #define OV5640_REG_SLAVE_ID             0x3100
  63 #define OV5640_REG_SCCB_SYS_CTRL1       0x3103
  64 #define OV5640_REG_SYS_ROOT_DIVIDER     0x3108
  65 #define OV5640_REG_AWB_R_GAIN           0x3400
  66 #define OV5640_REG_AWB_G_GAIN           0x3402
  67 #define OV5640_REG_AWB_B_GAIN           0x3404
  68 #define OV5640_REG_AWB_MANUAL_CTRL      0x3406
  69 #define OV5640_REG_AEC_PK_EXPOSURE_HI   0x3500
  70 #define OV5640_REG_AEC_PK_EXPOSURE_MED  0x3501
  71 #define OV5640_REG_AEC_PK_EXPOSURE_LO   0x3502
  72 #define OV5640_REG_AEC_PK_MANUAL        0x3503
  73 #define OV5640_REG_AEC_PK_REAL_GAIN     0x350a
  74 #define OV5640_REG_AEC_PK_VTS           0x350c
  75 #define OV5640_REG_TIMING_HS            0x3800
  76 #define OV5640_REG_TIMING_VS            0x3802
  77 #define OV5640_REG_TIMING_HW            0x3804
  78 #define OV5640_REG_TIMING_VH            0x3806
  79 #define OV5640_REG_TIMING_DVPHO         0x3808
  80 #define OV5640_REG_TIMING_DVPVO         0x380a
  81 #define OV5640_REG_TIMING_HTS           0x380c
  82 #define OV5640_REG_TIMING_VTS           0x380e
  83 #define OV5640_REG_TIMING_HOFFS         0x3810
  84 #define OV5640_REG_TIMING_VOFFS         0x3812
  85 #define OV5640_REG_TIMING_TC_REG20      0x3820
  86 #define OV5640_REG_TIMING_TC_REG21      0x3821
  87 #define OV5640_REG_AEC_CTRL00           0x3a00
  88 #define OV5640_REG_AEC_B50_STEP         0x3a08
  89 #define OV5640_REG_AEC_B60_STEP         0x3a0a
  90 #define OV5640_REG_AEC_CTRL0D           0x3a0d
  91 #define OV5640_REG_AEC_CTRL0E           0x3a0e
  92 #define OV5640_REG_AEC_CTRL0F           0x3a0f
  93 #define OV5640_REG_AEC_CTRL10           0x3a10
  94 #define OV5640_REG_AEC_CTRL11           0x3a11
  95 #define OV5640_REG_AEC_CTRL1B           0x3a1b
  96 #define OV5640_REG_AEC_CTRL1E           0x3a1e
  97 #define OV5640_REG_AEC_CTRL1F           0x3a1f
  98 #define OV5640_REG_HZ5060_CTRL00        0x3c00
  99 #define OV5640_REG_HZ5060_CTRL01        0x3c01
 100 #define OV5640_REG_SIGMADELTA_CTRL0C    0x3c0c
 101 #define OV5640_REG_FRAME_CTRL01         0x4202
 102 #define OV5640_REG_FORMAT_CONTROL00     0x4300
 103 #define OV5640_REG_VFIFO_HSIZE          0x4602
 104 #define OV5640_REG_VFIFO_VSIZE          0x4604
 105 #define OV5640_REG_JPG_MODE_SELECT      0x4713
 106 #define OV5640_REG_CCIR656_CTRL00       0x4730
 107 #define OV5640_REG_POLARITY_CTRL00      0x4740
 108 #define OV5640_REG_MIPI_CTRL00          0x4800
 109 #define OV5640_REG_DEBUG_MODE           0x4814
 110 #define OV5640_REG_PCLK_PERIOD          0x4837
 111 #define OV5640_REG_ISP_FORMAT_MUX_CTRL  0x501f
 112 #define OV5640_REG_PRE_ISP_TEST_SET1    0x503d
 113 #define OV5640_REG_SDE_CTRL0            0x5580
 114 #define OV5640_REG_SDE_CTRL1            0x5581
 115 #define OV5640_REG_SDE_CTRL3            0x5583
 116 #define OV5640_REG_SDE_CTRL4            0x5584
 117 #define OV5640_REG_SDE_CTRL5            0x5585
 118 #define OV5640_REG_AVG_READOUT          0x56a1
 119
 120 enum ov5640_mode_id {
 121         OV5640_MODE_QQVGA_160_120 = 0,
 122         OV5640_MODE_QCIF_176_144,
 123         OV5640_MODE_QVGA_320_240,
 124         OV5640_MODE_VGA_640_480,
 125         OV5640_MODE_NTSC_720_480,
 126         OV5640_MODE_PAL_720_576,
 127         OV5640_MODE_XGA_1024_768,
 128         OV5640_MODE_720P_1280_720,
 129         OV5640_MODE_1080P_1920_1080,
 130         OV5640_MODE_QSXGA_2592_1944,
 131         OV5640_NUM_MODES,
 132 };
 133
 134 enum ov5640_frame_rate {
 135         OV5640_15_FPS = 0,
 136         OV5640_30_FPS,
 137         OV5640_60_FPS,
 138         OV5640_NUM_FRAMERATES,
 139 };
 140
 141 enum ov5640_pixel_rate_id {
 142         OV5640_PIXEL_RATE_168M,
 143         OV5640_PIXEL_RATE_148M,
 144         OV5640_PIXEL_RATE_124M,
 145         OV5640_PIXEL_RATE_96M,
 146         OV5640_PIXEL_RATE_48M,
 147         OV5640_NUM_PIXEL_RATES,
 148 };
 149
 150 /*
 151  * The chip manual suggests 24/48/96/192 MHz pixel clocks.
 152  *
 153  * 192MHz exceeds the sysclk limits; use 168MHz as maximum pixel rate for
 154  * full resolution mode @15 FPS.
 155  */
 156 static const u32 ov5640_pixel_rates[] = {
 157         [OV5640_PIXEL_RATE_168M] = 168000000,
 158         [OV5640_PIXEL_RATE_148M] = 148000000,
 159         [OV5640_PIXEL_RATE_124M] = 124000000,
 160         [OV5640_PIXEL_RATE_96M] = 96000000,
 161         [OV5640_PIXEL_RATE_48M] = 48000000,
 162 };
 163
 164 /*
 165  * MIPI CSI-2 link frequencies.
 166  *
 167  * Derived from the above defined pixel rate for bpp = (8, 16, 24) and
 168  * data_lanes = (1, 2)
 169  *
 170  * link_freq = (pixel_rate * bpp) / (2 * data_lanes)
 171  */
 172 static const s64 ov5640_csi2_link_freqs[] = {
 173         992000000, 888000000, 768000000, 744000000, 672000000, 672000000,
 174         592000000, 592000000, 576000000, 576000000, 496000000, 496000000,
 175         384000000, 384000000, 384000000, 336000000, 296000000, 288000000,
 176         248000000, 192000000, 192000000, 192000000, 96000000,
 177 };
 178
 179 /* Link freq for default mode: UYVY 16 bpp, 2 data lanes. */
 180 #define OV5640_DEFAULT_LINK_FREQ        13
 181
 182 enum ov5640_format_mux {
 183         OV5640_FMT_MUX_YUV422 = 0,
 184         OV5640_FMT_MUX_RGB,
 185         OV5640_FMT_MUX_DITHER,
 186         OV5640_FMT_MUX_RAW_DPC,
 187         OV5640_FMT_MUX_SNR_RAW,
 188         OV5640_FMT_MUX_RAW_CIP,
 189 };
 190
 191 static const struct ov5640_pixfmt {
 192         u32 code;
 193         u32 colorspace;
 194         u8 bpp;
 195 } ov5640_formats[] = {
 196         {
 197                 .code = MEDIA_BUS_FMT_JPEG_1X8,
 198                 .colorspace = V4L2_COLORSPACE_JPEG,
 199                 .bpp = 16,
 200         }, {
 201                 .code = MEDIA_BUS_FMT_UYVY8_2X8,
 202                 .colorspace = V4L2_COLORSPACE_SRGB,
 203                 .bpp = 16,
 204         }, {
 205                 .code = MEDIA_BUS_FMT_UYVY8_1X16,
 206                 .colorspace = V4L2_COLORSPACE_SRGB,
 207                 .bpp = 16,
 208         }, {
 209                 .code = MEDIA_BUS_FMT_YUYV8_2X8,
 210                 .colorspace = V4L2_COLORSPACE_SRGB,
 211                 .bpp = 16,
 212         }, {
 213                 .code = MEDIA_BUS_FMT_YUYV8_1X16,
 214                 .colorspace = V4L2_COLORSPACE_SRGB,
 215                 .bpp = 16,
 216         }, {
 217                 .code = MEDIA_BUS_FMT_RGB565_2X8_LE,
 218                 .colorspace = V4L2_COLORSPACE_SRGB,
 219                 .bpp = 16,
 220         }, {
 221                 .code = MEDIA_BUS_FMT_RGB565_2X8_BE,
 222                 .colorspace = V4L2_COLORSPACE_SRGB,
 223                 .bpp = 16,
 224         }, {
 225                 .code = MEDIA_BUS_FMT_SBGGR8_1X8,
 226                 .colorspace = V4L2_COLORSPACE_SRGB,
 227                 .bpp = 8,
 228         }, {
 229                 .code = MEDIA_BUS_FMT_SGBRG8_1X8,
 230                 .colorspace = V4L2_COLORSPACE_SRGB,
 231                 .bpp = 8
 232         }, {
 233                 .code = MEDIA_BUS_FMT_SGRBG8_1X8,
 234                 .colorspace = V4L2_COLORSPACE_SRGB,
 235                 .bpp = 8,
 236         }, {
 237                 .code = MEDIA_BUS_FMT_SRGGB8_1X8,
 238                 .colorspace = V4L2_COLORSPACE_SRGB,
 239                 .bpp = 8,
 240         },
 241 };
 242
 243 static u32 ov5640_code_to_bpp(u32 code)
 244 {
 245         unsigned int i;
 246
 247         for (i = 0; i < ARRAY_SIZE(ov5640_formats); ++i) {
 248                 if (ov5640_formats[i].code == code)
 249                         return ov5640_formats[i].bpp;
 250         }
 251
 252         return 0;
 253 }
 254
 255 /*
 256  * FIXME: remove this when a subdev API becomes available
 257  * to set the MIPI CSI-2 virtual channel.
 258  */
 259 static unsigned int virtual_channel;
 260 module_param(virtual_channel, uint, 0444);
 261 MODULE_PARM_DESC(virtual_channel,
 262                  "MIPI CSI-2 virtual channel (0..3), default 0");
 263
 264 static const int ov5640_framerates[] = {
 265         [OV5640_15_FPS] = 15,
 266         [OV5640_30_FPS] = 30,
 267         [OV5640_60_FPS] = 60,
 268 };
 269
 270 /* regulator supplies */
 271 static const char * const ov5640_supply_name[] = {
 272         "DOVDD", /* Digital I/O (1.8V) supply */
 273         "AVDD",  /* Analog (2.8V) supply */
 274         "DVDD",  /* Digital Core (1.5V) supply */
 275 };
 276
 277 #define OV5640_NUM_SUPPLIES ARRAY_SIZE(ov5640_supply_name)
 278
 279 /*
 280  * Image size under 1280 * 960 are SUBSAMPLING
 281  * Image size upper 1280 * 960 are SCALING
 282  */
 283 enum ov5640_downsize_mode {
 284         SUBSAMPLING,
 285         SCALING,
 286 };
 287
 288 struct reg_value {
 289         u16 reg_addr;
 290         u8 val;
 291         u8 mask;
 292         u32 delay_ms;
 293 };
 294
 295 struct ov5640_timings {
 296         /* Analog crop rectangle. */
 297         struct v4l2_rect analog_crop;
 298         /* Visibile crop: from analog crop top-left corner. */
 299         struct v4l2_rect crop;
 300         /* Total pixels per line: width + fixed hblank. */
 301         u32 htot;
 302         /* Default vertical blanking: frame height = height + vblank. */
 303         u32 vblank_def;
 304 };
 305
 306 struct ov5640_mode_info {
 307         enum ov5640_mode_id id;
 308         enum ov5640_downsize_mode dn_mode;
 309         enum ov5640_pixel_rate_id pixel_rate;
 310
 311         unsigned int width;
 312         unsigned int height;
 313
 314         struct ov5640_timings dvp_timings;
 315         struct ov5640_timings csi2_timings;
 316
 317         const struct reg_value *reg_data;
 318         u32 reg_data_size;
 319
 320         /* Used by s_frame_interval only. */
 321         u32 max_fps;
 322 };
 323
 324 struct ov5640_ctrls {
 325         struct v4l2_ctrl_handler handler;
 326         struct v4l2_ctrl *pixel_rate;
 327         struct v4l2_ctrl *link_freq;
 328         struct v4l2_ctrl *hblank;
 329         struct v4l2_ctrl *vblank;
 330         struct {
 331                 struct v4l2_ctrl *auto_exp;
 332                 struct v4l2_ctrl *exposure;
 333         };
 334         struct {
 335                 struct v4l2_ctrl *auto_wb;
 336                 struct v4l2_ctrl *blue_balance;
 337                 struct v4l2_ctrl *red_balance;
 338         };
 339         struct {
 340                 struct v4l2_ctrl *auto_gain;
 341                 struct v4l2_ctrl *gain;
 342         };
 343         struct v4l2_ctrl *brightness;
 344         struct v4l2_ctrl *light_freq;
 345         struct v4l2_ctrl *saturation;
 346         struct v4l2_ctrl *contrast;
 347         struct v4l2_ctrl *hue;
 348         struct v4l2_ctrl *test_pattern;
 349         struct v4l2_ctrl *hflip;
 350         struct v4l2_ctrl *vflip;
 351 };
 352
 353 struct ov5640_dev {
 354         struct i2c_client *i2c_client;
 355         struct v4l2_subdev sd;
 356         struct media_pad pad;
 357         struct v4l2_fwnode_endpoint ep; /* the parsed DT endpoint info */
 358         struct clk *xclk; /* system clock to OV5640 */
 359         u32 xclk_freq;
 360
 361         struct regulator_bulk_data supplies[OV5640_NUM_SUPPLIES];
 362         struct gpio_desc *reset_gpio;
 363         struct gpio_desc *pwdn_gpio;
 364         bool   upside_down;
 365
 366         /* lock to protect all members below */
 367         struct mutex lock;
 368
 369         int power_count;
 370
 371         struct v4l2_mbus_framefmt fmt;
 372         bool pending_fmt_change;
 373
 374         const struct ov5640_mode_info *current_mode;
 375         const struct ov5640_mode_info *last_mode;
 376         enum ov5640_frame_rate current_fr;
 377         struct v4l2_fract frame_interval;
 378         s64 current_link_freq;
 379
 380         struct ov5640_ctrls ctrls;
 381
 382         u32 prev_sysclk, prev_hts;
 383         u32 ae_low, ae_high, ae_target;
 384
 385         bool pending_mode_change;
 386         bool streaming;
 387 };
 388
 389 static inline struct ov5640_dev *to_ov5640_dev(struct v4l2_subdev *sd)
 390 {
 391         return container_of(sd, struct ov5640_dev, sd);
 392 }
 393
 394 static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
 395 {
 396         return &container_of(ctrl->handler, struct ov5640_dev,
 397                              ctrls.handler)->sd;
 398 }
 399
 400 static inline bool ov5640_is_csi2(const struct ov5640_dev *sensor)
 401 {
 402         return sensor->ep.bus_type == V4L2_MBUS_CSI2_DPHY;
 403 }
 404
 405 /*
 406  * FIXME: all of these register tables are likely filled with
 407  * entries that set the register to their power-on default values,
 408  * and which are otherwise not touched by this driver. Those entries
 409  * should be identified and removed to speed register load time
 410  * over i2c.
 411  */
 412 /* YUV422 UYVY VGA@30fps */
 413 static const struct reg_value ov5640_init_setting[] = {
 414         {0x3103, 0x11, 0, 0}, {0x3008, 0x82, 0, 5}, {0x3008, 0x42, 0, 0},
 415         {0x3103, 0x03, 0, 0}, {0x3630, 0x36, 0, 0},
 416         {0x3631, 0x0e, 0, 0}, {0x3632, 0xe2, 0, 0}, {0x3633, 0x12, 0, 0},
 417         {0x3621, 0xe0, 0, 0}, {0x3704, 0xa0, 0, 0}, {0x3703, 0x5a, 0, 0},
 418         {0x3715, 0x78, 0, 0}, {0x3717, 0x01, 0, 0}, {0x370b, 0x60, 0, 0},
 419         {0x3705, 0x1a, 0, 0}, {0x3905, 0x02, 0, 0}, {0x3906, 0x10, 0, 0},
 420         {0x3901, 0x0a, 0, 0}, {0x3731, 0x12, 0, 0}, {0x3600, 0x08, 0, 0},
 421         {0x3601, 0x33, 0, 0}, {0x302d, 0x60, 0, 0}, {0x3620, 0x52, 0, 0},
 422         {0x371b, 0x20, 0, 0}, {0x471c, 0x50, 0, 0}, {0x3a13, 0x43, 0, 0},
 423         {0x3a18, 0x00, 0, 0}, {0x3a19, 0xf8, 0, 0}, {0x3635, 0x13, 0, 0},
 424         {0x3636, 0x03, 0, 0}, {0x3634, 0x40, 0, 0}, {0x3622, 0x01, 0, 0},
 425         {0x3c01, 0xa4, 0, 0}, {0x3c04, 0x28, 0, 0}, {0x3c05, 0x98, 0, 0},
 426         {0x3c06, 0x00, 0, 0}, {0x3c07, 0x08, 0, 0}, {0x3c08, 0x00, 0, 0},
 427         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 428         {0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
 429         {0x3815, 0x31, 0, 0},
 430         {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
 431         {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
 432         {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
 433         {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
 434         {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
 435         {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x3000, 0x00, 0, 0},
 436         {0x3002, 0x1c, 0, 0}, {0x3004, 0xff, 0, 0}, {0x3006, 0xc3, 0, 0},
 437         {0x302e, 0x08, 0, 0}, {0x4300, 0x3f, 0, 0},
 438         {0x501f, 0x00, 0, 0}, {0x4407, 0x04, 0, 0},
 439         {0x440e, 0x00, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
 440         {0x4837, 0x0a, 0, 0}, {0x3824, 0x02, 0, 0},
 441         {0x5000, 0xa7, 0, 0}, {0x5001, 0xa3, 0, 0}, {0x5180, 0xff, 0, 0},
 442         {0x5181, 0xf2, 0, 0}, {0x5182, 0x00, 0, 0}, {0x5183, 0x14, 0, 0},
 443         {0x5184, 0x25, 0, 0}, {0x5185, 0x24, 0, 0}, {0x5186, 0x09, 0, 0},
 444         {0x5187, 0x09, 0, 0}, {0x5188, 0x09, 0, 0}, {0x5189, 0x88, 0, 0},
 445         {0x518a, 0x54, 0, 0}, {0x518b, 0xee, 0, 0}, {0x518c, 0xb2, 0, 0},
 446         {0x518d, 0x50, 0, 0}, {0x518e, 0x34, 0, 0}, {0x518f, 0x6b, 0, 0},
 447         {0x5190, 0x46, 0, 0}, {0x5191, 0xf8, 0, 0}, {0x5192, 0x04, 0, 0},
 448         {0x5193, 0x70, 0, 0}, {0x5194, 0xf0, 0, 0}, {0x5195, 0xf0, 0, 0},
 449         {0x5196, 0x03, 0, 0}, {0x5197, 0x01, 0, 0}, {0x5198, 0x04, 0, 0},
 450         {0x5199, 0x6c, 0, 0}, {0x519a, 0x04, 0, 0}, {0x519b, 0x00, 0, 0},
 451         {0x519c, 0x09, 0, 0}, {0x519d, 0x2b, 0, 0}, {0x519e, 0x38, 0, 0},
 452         {0x5381, 0x1e, 0, 0}, {0x5382, 0x5b, 0, 0}, {0x5383, 0x08, 0, 0},
 453         {0x5384, 0x0a, 0, 0}, {0x5385, 0x7e, 0, 0}, {0x5386, 0x88, 0, 0},
 454         {0x5387, 0x7c, 0, 0}, {0x5388, 0x6c, 0, 0}, {0x5389, 0x10, 0, 0},
 455         {0x538a, 0x01, 0, 0}, {0x538b, 0x98, 0, 0}, {0x5300, 0x08, 0, 0},
 456         {0x5301, 0x30, 0, 0}, {0x5302, 0x10, 0, 0}, {0x5303, 0x00, 0, 0},
 457         {0x5304, 0x08, 0, 0}, {0x5305, 0x30, 0, 0}, {0x5306, 0x08, 0, 0},
 458         {0x5307, 0x16, 0, 0}, {0x5309, 0x08, 0, 0}, {0x530a, 0x30, 0, 0},
 459         {0x530b, 0x04, 0, 0}, {0x530c, 0x06, 0, 0}, {0x5480, 0x01, 0, 0},
 460         {0x5481, 0x08, 0, 0}, {0x5482, 0x14, 0, 0}, {0x5483, 0x28, 0, 0},
 461         {0x5484, 0x51, 0, 0}, {0x5485, 0x65, 0, 0}, {0x5486, 0x71, 0, 0},
 462         {0x5487, 0x7d, 0, 0}, {0x5488, 0x87, 0, 0}, {0x5489, 0x91, 0, 0},
 463         {0x548a, 0x9a, 0, 0}, {0x548b, 0xaa, 0, 0}, {0x548c, 0xb8, 0, 0},
 464         {0x548d, 0xcd, 0, 0}, {0x548e, 0xdd, 0, 0}, {0x548f, 0xea, 0, 0},
 465         {0x5490, 0x1d, 0, 0}, {0x5580, 0x02, 0, 0}, {0x5583, 0x40, 0, 0},
 466         {0x5584, 0x10, 0, 0}, {0x5589, 0x10, 0, 0}, {0x558a, 0x00, 0, 0},
 467         {0x558b, 0xf8, 0, 0}, {0x5800, 0x23, 0, 0}, {0x5801, 0x14, 0, 0},
 468         {0x5802, 0x0f, 0, 0}, {0x5803, 0x0f, 0, 0}, {0x5804, 0x12, 0, 0},
 469         {0x5805, 0x26, 0, 0}, {0x5806, 0x0c, 0, 0}, {0x5807, 0x08, 0, 0},
 470         {0x5808, 0x05, 0, 0}, {0x5809, 0x05, 0, 0}, {0x580a, 0x08, 0, 0},
 471         {0x580b, 0x0d, 0, 0}, {0x580c, 0x08, 0, 0}, {0x580d, 0x03, 0, 0},
 472         {0x580e, 0x00, 0, 0}, {0x580f, 0x00, 0, 0}, {0x5810, 0x03, 0, 0},
 473         {0x5811, 0x09, 0, 0}, {0x5812, 0x07, 0, 0}, {0x5813, 0x03, 0, 0},
 474         {0x5814, 0x00, 0, 0}, {0x5815, 0x01, 0, 0}, {0x5816, 0x03, 0, 0},
 475         {0x5817, 0x08, 0, 0}, {0x5818, 0x0d, 0, 0}, {0x5819, 0x08, 0, 0},
 476         {0x581a, 0x05, 0, 0}, {0x581b, 0x06, 0, 0}, {0x581c, 0x08, 0, 0},
 477         {0x581d, 0x0e, 0, 0}, {0x581e, 0x29, 0, 0}, {0x581f, 0x17, 0, 0},
 478         {0x5820, 0x11, 0, 0}, {0x5821, 0x11, 0, 0}, {0x5822, 0x15, 0, 0},
 479         {0x5823, 0x28, 0, 0}, {0x5824, 0x46, 0, 0}, {0x5825, 0x26, 0, 0},
 480         {0x5826, 0x08, 0, 0}, {0x5827, 0x26, 0, 0}, {0x5828, 0x64, 0, 0},
 481         {0x5829, 0x26, 0, 0}, {0x582a, 0x24, 0, 0}, {0x582b, 0x22, 0, 0},
 482         {0x582c, 0x24, 0, 0}, {0x582d, 0x24, 0, 0}, {0x582e, 0x06, 0, 0},
 483         {0x582f, 0x22, 0, 0}, {0x5830, 0x40, 0, 0}, {0x5831, 0x42, 0, 0},
 484         {0x5832, 0x24, 0, 0}, {0x5833, 0x26, 0, 0}, {0x5834, 0x24, 0, 0},
 485         {0x5835, 0x22, 0, 0}, {0x5836, 0x22, 0, 0}, {0x5837, 0x26, 0, 0},
 486         {0x5838, 0x44, 0, 0}, {0x5839, 0x24, 0, 0}, {0x583a, 0x26, 0, 0},
 487         {0x583b, 0x28, 0, 0}, {0x583c, 0x42, 0, 0}, {0x583d, 0xce, 0, 0},
 488         {0x5025, 0x00, 0, 0}, {0x3a0f, 0x30, 0, 0}, {0x3a10, 0x28, 0, 0},
 489         {0x3a1b, 0x30, 0, 0}, {0x3a1e, 0x26, 0, 0}, {0x3a11, 0x60, 0, 0},
 490         {0x3a1f, 0x14, 0, 0}, {0x3008, 0x02, 0, 0}, {0x3c00, 0x04, 0, 300},
 491 };
 492
 493 static const struct reg_value ov5640_setting_low_res[] = {
 494         {0x3c07, 0x08, 0, 0},
 495         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 496         {0x3814, 0x31, 0, 0},
 497         {0x3815, 0x31, 0, 0},
 498         {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
 499         {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
 500         {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
 501         {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
 502         {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
 503         {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
 504         {0x4407, 0x04, 0, 0}, {0x5001, 0xa3, 0, 0},
 505 };
 506
 507 static const struct reg_value ov5640_setting_720P_1280_720[] = {
 508         {0x3c07, 0x07, 0, 0},
 509         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 510         {0x3814, 0x31, 0, 0},
 511         {0x3815, 0x31, 0, 0},
 512         {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
 513         {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x02, 0, 0},
 514         {0x3a03, 0xe4, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0xbc, 0, 0},
 515         {0x3a0a, 0x01, 0, 0}, {0x3a0b, 0x72, 0, 0}, {0x3a0e, 0x01, 0, 0},
 516         {0x3a0d, 0x02, 0, 0}, {0x3a14, 0x02, 0, 0}, {0x3a15, 0xe4, 0, 0},
 517         {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
 518         {0x4407, 0x04, 0, 0}, {0x460b, 0x37, 0, 0}, {0x460c, 0x20, 0, 0},
 519         {0x3824, 0x04, 0, 0}, {0x5001, 0x83, 0, 0},
 520 };
 521
 522 static const struct reg_value ov5640_setting_1080P_1920_1080[] = {
 523         {0x3c07, 0x08, 0, 0},
 524         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 525         {0x3814, 0x11, 0, 0},
 526         {0x3815, 0x11, 0, 0},
 527         {0x3618, 0x04, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x21, 0, 0},
 528         {0x3709, 0x12, 0, 0}, {0x370c, 0x00, 0, 0}, {0x3a02, 0x03, 0, 0},
 529         {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
 530         {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
 531         {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
 532         {0x4001, 0x02, 0, 0}, {0x4004, 0x06, 0, 0},
 533         {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
 534         {0x3824, 0x02, 0, 0}, {0x5001, 0x83, 0, 0},
 535         {0x3c07, 0x07, 0, 0}, {0x3c08, 0x00, 0, 0},
 536         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 537         {0x3612, 0x2b, 0, 0}, {0x3708, 0x64, 0, 0},
 538         {0x3a02, 0x04, 0, 0}, {0x3a03, 0x60, 0, 0}, {0x3a08, 0x01, 0, 0},
 539         {0x3a09, 0x50, 0, 0}, {0x3a0a, 0x01, 0, 0}, {0x3a0b, 0x18, 0, 0},
 540         {0x3a0e, 0x03, 0, 0}, {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x04, 0, 0},
 541         {0x3a15, 0x60, 0, 0}, {0x4407, 0x04, 0, 0},
 542         {0x460b, 0x37, 0, 0}, {0x460c, 0x20, 0, 0}, {0x3824, 0x04, 0, 0},
 543         {0x4005, 0x1a, 0, 0},
 544 };
 545
 546 static const struct reg_value ov5640_setting_QSXGA_2592_1944[] = {
 547         {0x3c07, 0x08, 0, 0},
 548         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 549         {0x3814, 0x11, 0, 0},
 550         {0x3815, 0x11, 0, 0},
 551         {0x3618, 0x04, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x21, 0, 0},
 552         {0x3709, 0x12, 0, 0}, {0x370c, 0x00, 0, 0}, {0x3a02, 0x03, 0, 0},
 553         {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
 554         {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
 555         {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
 556         {0x4001, 0x02, 0, 0}, {0x4004, 0x06, 0, 0},
 557         {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
 558         {0x3824, 0x02, 0, 0}, {0x5001, 0x83, 0, 70},
 559 };
 560
 561 static const struct ov5640_mode_info ov5640_mode_data[OV5640_NUM_MODES] = {
 562         {
 563                 /* 160x120 */
 564                 .id             = OV5640_MODE_QQVGA_160_120,
 565                 .dn_mode        = SUBSAMPLING,
 566                 .pixel_rate     = OV5640_PIXEL_RATE_48M,
 567                 .width          = 160,
 568                 .height         = 120,
 569                 .dvp_timings = {
 570                         .analog_crop = {
 571                                 .left   = 0,
 572                                 .top    = 4,
 573                                 .width  = 2624,
 574                                 .height = 1944,
 575                         },
 576                         .crop = {
 577                                 .left   = 16,
 578                                 .top    = 6,
 579                                 .width  = 160,
 580                                 .height = 120,
 581                         },
 582                         .htot           = 1896,
 583                         .vblank_def     = 864,
 584                 },
 585                 .csi2_timings = {
 586                         /* Feed the full valid pixel array to the ISP. */
 587                         .analog_crop = {
 588                                 .left   = OV5640_PIXEL_ARRAY_LEFT,
 589                                 .top    = OV5640_PIXEL_ARRAY_TOP,
 590                                 .width  = OV5640_PIXEL_ARRAY_WIDTH,
 591                                 .height = OV5640_PIXEL_ARRAY_HEIGHT,
 592                         },
 593                         /* Maintain a minimum processing margin. */
 594                         .crop = {
 595                                 .left   = 2,
 596                                 .top    = 4,
 597                                 .width  = 160,
 598                                 .height = 120,
 599                         },
 600                         .htot           = 1896,
 601                         .vblank_def     = 864,
 602                 },
 603                 .reg_data       = ov5640_setting_low_res,
 604                 .reg_data_size  = ARRAY_SIZE(ov5640_setting_low_res),
 605                 .max_fps        = OV5640_30_FPS
 606         }, {
 607                 /* 176x144 */
 608                 .id             = OV5640_MODE_QCIF_176_144,
 609                 .dn_mode        = SUBSAMPLING,
 610                 .pixel_rate     = OV5640_PIXEL_RATE_48M,
 611                 .width          = 176,
 612                 .height         = 144,
 613                 .dvp_timings = {
 614                         .analog_crop = {
 615                                 .left   = 0,
 616                                 .top    = 4,
 617                                 .width  = 2624,
 618                                 .height = 1944,
 619                         },
 620                         .crop = {
 621                                 .left   = 16,
 622                                 .top    = 6,
 623                                 .width  = 176,
 624                                 .height = 144,
 625                         },
 626                         .htot           = 1896,
 627                         .vblank_def     = 840,
 628                 },
 629                 .csi2_timings = {
 630                         /* Feed the full valid pixel array to the ISP. */
 631                         .analog_crop = {
 632                                 .left   = OV5640_PIXEL_ARRAY_LEFT,
 633                                 .top    = OV5640_PIXEL_ARRAY_TOP,
 634                                 .width  = OV5640_PIXEL_ARRAY_WIDTH,
 635                                 .height = OV5640_PIXEL_ARRAY_HEIGHT,
 636                         },
 637                         /* Maintain a minimum processing margin. */
 638                         .crop = {
 639                                 .left   = 2,
 640                                 .top    = 4,
 641                                 .width  = 176,
 642                                 .height = 144,
 643                         },
 644                         .htot           = 1896,
 645                         .vblank_def     = 840,
 646                 },
 647                 .reg_data       = ov5640_setting_low_res,
 648                 .reg_data_size  = ARRAY_SIZE(ov5640_setting_low_res),
 649                 .max_fps        = OV5640_30_FPS
 650         }, {
 651                 /* 320x240 */
 652                 .id             = OV5640_MODE_QVGA_320_240,
 653                 .dn_mode        = SUBSAMPLING,
 654                 .width          = 320,
 655                 .height         = 240,
 656                 .pixel_rate     = OV5640_PIXEL_RATE_48M,
 657                 .dvp_timings = {
 658                         .analog_crop = {
 659                                 .left   = 0,
 660                                 .top    = 4,
 661                                 .width  = 2624,
 662                                 .height = 1944,
 663                         },
 664                         .crop = {
 665                                 .left   = 16,
 666                                 .top    = 6,
 667                                 .width  = 320,
 668                                 .height = 240,
 669                         },
 670                         .htot           = 1896,
 671                         .vblank_def     = 744,
 672                 },
 673                 .csi2_timings = {
 674                         /* Feed the full valid pixel array to the ISP. */
 675                         .analog_crop = {
 676                                 .left   = OV5640_PIXEL_ARRAY_LEFT,
 677                                 .top    = OV5640_PIXEL_ARRAY_TOP,
 678                                 .width  = OV5640_PIXEL_ARRAY_WIDTH,
 679                                 .height = OV5640_PIXEL_ARRAY_HEIGHT,
 680                         },
 681                         /* Maintain a minimum processing margin. */
 682                         .crop = {
 683                                 .left   = 2,
 684                                 .top    = 4,
 685                                 .width  = 320,
 686                                 .height = 240,
 687                         },
 688                         .htot           = 1896,
 689                         .vblank_def     = 744,
 690                 },
 691                 .reg_data       = ov5640_setting_low_res,
 692                 .reg_data_size  = ARRAY_SIZE(ov5640_setting_low_res),
 693                 .max_fps        = OV5640_30_FPS
 694         }, {
 695                 /* 640x480 */
 696                 .id             = OV5640_MODE_VGA_640_480,
 697                 .dn_mode        = SUBSAMPLING,
 698                 .pixel_rate     = OV5640_PIXEL_RATE_48M,
 699                 .width          = 640,
 700                 .height         = 480,
 701                 .dvp_timings = {
 702                         .analog_crop = {
 703                                 .left   = 0,
 704                                 .top    = 4,
 705                                 .width  = 2624,
 706                                 .height = 1944,
 707                         },
 708                         .crop = {
 709                                 .left   = 16,
 710                                 .top    = 6,
 711                                 .width  = 640,
 712                                 .height = 480,
 713                         },
 714                         .htot           = 1896,
 715                         .vblank_def     = 600,
 716                 },
 717                 .csi2_timings = {
 718                         /* Feed the full valid pixel array to the ISP. */
 719                         .analog_crop = {
 720                                 .left   = OV5640_PIXEL_ARRAY_LEFT,
 721                                 .top    = OV5640_PIXEL_ARRAY_TOP,
 722                                 .width  = OV5640_PIXEL_ARRAY_WIDTH,
 723                                 .height = OV5640_PIXEL_ARRAY_HEIGHT,
 724                         },
 725                         /* Maintain a minimum processing margin. */
 726                         .crop = {
 727                                 .left   = 2,
 728                                 .top    = 4,
 729                                 .width  = 640,
 730                                 .height = 480,
 731                         },
 732                         .htot           = 1896,
 733                         .vblank_def     = 600,
 734                 },
 735                 .reg_data       = ov5640_setting_low_res,
 736                 .reg_data_size  = ARRAY_SIZE(ov5640_setting_low_res),
 737                 .max_fps        = OV5640_60_FPS
 738         }, {
 739                 /* 720x480 */
 740                 .id             = OV5640_MODE_NTSC_720_480,
 741                 .dn_mode        = SUBSAMPLING,
 742                 .width          = 720,
 743                 .height         = 480,
 744                 .pixel_rate     = OV5640_PIXEL_RATE_96M,
 745                 .dvp_timings = {
 746                         .analog_crop = {
 747                                 .left   = 0,
 748                                 .top    = 4,
 749                                 .width  = 2624,
 750                                 .height = 1944,
 751                         },
 752                         .crop = {
 753                                 .left   = 56,
 754                                 .top    = 60,
 755                                 .width  = 720,
 756                                 .height = 480,
 757                         },
 758                         .htot           = 1896,
 759                         .vblank_def     = 504,
 760                 },
 761                 .csi2_timings = {
 762                         /* Feed the full valid pixel array to the ISP. */
 763                         .analog_crop = {
 764                                 .left   = OV5640_PIXEL_ARRAY_LEFT,
 765                                 .top    = OV5640_PIXEL_ARRAY_TOP,
 766                                 .width  = OV5640_PIXEL_ARRAY_WIDTH,
 767                                 .height = OV5640_PIXEL_ARRAY_HEIGHT,
 768                         },
 769                         .crop = {
 770                                 .left   = 56,
 771                                 .top    = 60,
 772                                 .width  = 720,
 773                                 .height = 480,
 774                         },
 775                         .htot           = 1896,
 776                         .vblank_def     = 504,
 777                 },
 778                 .reg_data       = ov5640_setting_low_res,
 779                 .reg_data_size  = ARRAY_SIZE(ov5640_setting_low_res),
 780                 .max_fps        = OV5640_30_FPS
 781         }, {
 782                 /* 720x576 */
 783                 .id             = OV5640_MODE_PAL_720_576,
 784                 .dn_mode        = SUBSAMPLING,
 785                 .width          = 720,
 786                 .height         = 576,
 787                 .pixel_rate     = OV5640_PIXEL_RATE_96M,
 788                 .dvp_timings = {
 789                         .analog_crop = {
 790                                 .left   = 0,
 791                                 .top    = 4,
 792                                 .width  = 2624,
 793                                 .height = 1944,
 794                         },
 795                         .crop = {
 796                                 .left   = 56,
 797                                 .top    = 6,
 798                                 .width  = 720,
 799                                 .height = 576,
 800                         },
 801                         .htot           = 1896,
 802                         .vblank_def     = 408,
 803                 },
 804                 .csi2_timings = {
 805                         /* Feed the full valid pixel array to the ISP. */
 806                         .analog_crop = {
 807                                 .left   = OV5640_PIXEL_ARRAY_LEFT,
 808                                 .top    = OV5640_PIXEL_ARRAY_TOP,
 809                                 .width  = OV5640_PIXEL_ARRAY_WIDTH,
 810                                 .height = OV5640_PIXEL_ARRAY_HEIGHT,
 811                         },
 812                         .crop = {
 813                                 .left   = 56,
 814                                 .top    = 6,
 815                                 .width  = 720,
 816                                 .height = 576,
 817                         },
 818                         .htot           = 1896,
 819                         .vblank_def     = 408,
 820                 },
 821                 .reg_data       = ov5640_setting_low_res,
 822                 .reg_data_size  = ARRAY_SIZE(ov5640_setting_low_res),
 823                 .max_fps        = OV5640_30_FPS
 824         }, {
 825                 /* 1024x768 */
 826                 .id             = OV5640_MODE_XGA_1024_768,
 827                 .dn_mode        = SUBSAMPLING,
 828                 .pixel_rate     = OV5640_PIXEL_RATE_96M,
 829                 .width          = 1024,
 830                 .height         = 768,
 831                 .dvp_timings = {
 832                         .analog_crop = {
 833                                 .left   = 0,
 834                                 .top    = 4,
 835                                 .width  = 2624,
 836                                 .height = 1944,
 837                         },
 838                         .crop = {
 839                                 .left   = 16,
 840                                 .top    = 6,
 841                                 .width  = 1024,
 842                                 .height = 768,
 843                         },
 844                         .htot           = 1896,
 845                         .vblank_def     = 312,
 846                 },
 847                 .csi2_timings = {
 848                         .analog_crop = {
 849                                 .left   = 0,
 850                                 .top    = 4,
 851                                 .width  = OV5640_NATIVE_WIDTH,
 852                                 .height = OV5640_PIXEL_ARRAY_HEIGHT,
 853                         },
 854                         .crop = {
 855                                 .left   = 16,
 856                                 .top    = 6,
 857                                 .width  = 1024,
 858                                 .height = 768,
 859                         },
 860                         .htot           = 1896,
 861                         .vblank_def     = 312,
 862                 },
 863                 .reg_data       = ov5640_setting_low_res,
 864                 .reg_data_size  = ARRAY_SIZE(ov5640_setting_low_res),
 865                 .max_fps        = OV5640_30_FPS
 866         }, {
 867                 /* 1280x720 */
 868                 .id             = OV5640_MODE_720P_1280_720,
 869                 .dn_mode        = SUBSAMPLING,
 870                 .pixel_rate     = OV5640_PIXEL_RATE_124M,
 871                 .width          = 1280,
 872                 .height         = 720,
 873                 .dvp_timings = {
 874                         .analog_crop = {
 875                                 .left   = 0,
 876                                 .top    = 250,
 877                                 .width  = 2624,
 878                                 .height = 1456,
 879                         },
 880                         .crop = {
 881                                 .left   = 16,
 882                                 .top    = 4,
 883                                 .width  = 1280,
 884                                 .height = 720,
 885                         },
 886                         .htot           = 1892,
 887                         .vblank_def     = 20,
 888                 },
 889                 .csi2_timings = {
 890                         .analog_crop = {
 891                                 .left   = 0,
 892                                 .top    = 250,
 893                                 .width  = 2624,
 894                                 .height = 1456,
 895                         },
 896                         .crop = {
 897                                 .left   = 16,
 898                                 .top    = 4,
 899                                 .width  = 1280,
 900                                 .height = 720,
 901                         },
 902                         .htot           = 1892,
 903                         .vblank_def     = 20,
 904                 },
 905                 .reg_data       = ov5640_setting_720P_1280_720,
 906                 .reg_data_size  = ARRAY_SIZE(ov5640_setting_720P_1280_720),
 907                 .max_fps        = OV5640_30_FPS
 908         }, {
 909                 /* 1920x1080 */
 910                 .id             = OV5640_MODE_1080P_1920_1080,
 911                 .dn_mode        = SCALING,
 912                 .pixel_rate     = OV5640_PIXEL_RATE_148M,
 913                 .width          = 1920,
 914                 .height         = 1080,
 915                 .dvp_timings = {
 916                         .analog_crop = {
 917                                 .left   = 336,
 918                                 .top    = 434,
 919                                 .width  = 1952,
 920                                 .height = 1088,
 921                         },
 922                         .crop = {
 923                                 .left   = 16,
 924                                 .top    = 4,
 925                                 .width  = 1920,
 926                                 .height = 1080,
 927                         },
 928                         .htot           = 2500,
 929                         .vblank_def     = 40,
 930                 },
 931                 .csi2_timings = {
 932                         /* Crop the full valid pixel array in the center. */
 933                         .analog_crop = {
 934                                 .left   = 336,
 935                                 .top    = 434,
 936                                 .width  = 1952,
 937                                 .height = 1088,
 938                         },
 939                         /* Maintain a larger processing margins. */
 940                         .crop = {
 941                                 .left   = 16,
 942                                 .top    = 4,
 943                                 .width  = 1920,
 944                                 .height = 1080,
 945                         },
 946                         .htot           = 2500,
 947                         .vblank_def     = 40,
 948                 },
 949                 .reg_data       = ov5640_setting_1080P_1920_1080,
 950                 .reg_data_size  = ARRAY_SIZE(ov5640_setting_1080P_1920_1080),
 951                 .max_fps        = OV5640_30_FPS
 952         }, {
 953                 /* 2592x1944 */
 954                 .id             = OV5640_MODE_QSXGA_2592_1944,
 955                 .dn_mode        = SCALING,
 956                 .pixel_rate     = OV5640_PIXEL_RATE_168M,
 957                 .width          = OV5640_PIXEL_ARRAY_WIDTH,
 958                 .height         = OV5640_PIXEL_ARRAY_HEIGHT,
 959                 .dvp_timings = {
 960                         .analog_crop = {
 961                                 .left   = 0,
 962                                 .top    = 0,
 963                                 .width  = 2624,
 964                                 .height = 1952,
 965                         },
 966                         .crop = {
 967                                 .left   = 16,
 968                                 .top    = 4,
 969                                 .width  = 2592,
 970                                 .height = 1944,
 971                         },
 972                         .htot           = 2844,
 973                         .vblank_def     = 24,
 974                 },
 975                 .csi2_timings = {
 976                         /* Give more processing margin to full resolution. */
 977                         .analog_crop = {
 978                                 .left   = 0,
 979                                 .top    = 0,
 980                                 .width  = OV5640_NATIVE_WIDTH,
 981                                 .height = 1952,
 982                         },
 983                         .crop = {
 984                                 .left   = 16,
 985                                 .top    = 4,
 986                                 .width  = 2592,
 987                                 .height = 1944,
 988                         },
 989                         .htot           = 2844,
 990                         .vblank_def     = 24,
 991                 },
 992                 .reg_data       = ov5640_setting_QSXGA_2592_1944,
 993                 .reg_data_size  = ARRAY_SIZE(ov5640_setting_QSXGA_2592_1944),
 994                 .max_fps        = OV5640_15_FPS
 995         },
 996 };
 997
 998 static const struct ov5640_timings *
 999 ov5640_timings(const struct ov5640_dev *sensor,
1000                const struct ov5640_mode_info *mode)
1001 {
1002         if (ov5640_is_csi2(sensor))
1003                 return &mode->csi2_timings;
1004
1005         return &mode->dvp_timings;
1006 }
1007
1008 static int ov5640_init_slave_id(struct ov5640_dev *sensor)
1009 {
1010         struct i2c_client *client = sensor->i2c_client;
1011         struct i2c_msg msg;
1012         u8 buf[3];
1013         int ret;
1014
1015         if (client->addr == OV5640_DEFAULT_SLAVE_ID)
1016                 return 0;
1017
1018         buf[0] = OV5640_REG_SLAVE_ID >> 8;
1019         buf[1] = OV5640_REG_SLAVE_ID & 0xff;
1020         buf[2] = client->addr << 1;
1021
1022         msg.addr = OV5640_DEFAULT_SLAVE_ID;
1023         msg.flags = 0;
1024         msg.buf = buf;
1025         msg.len = sizeof(buf);
1026
1027         ret = i2c_transfer(client->adapter, &msg, 1);
1028         if (ret < 0) {
1029                 dev_err(&client->dev, "%s: failed with %d\n", __func__, ret);
1030                 return ret;
1031         }
1032
1033         return 0;
1034 }
1035
1036 static int ov5640_write_reg(struct ov5640_dev *sensor, u16 reg, u8 val)
1037 {
1038         struct i2c_client *client = sensor->i2c_client;
1039         struct i2c_msg msg;
1040         u8 buf[3];
1041         int ret;
1042
1043         buf[0] = reg >> 8;
1044         buf[1] = reg & 0xff;
1045         buf[2] = val;
1046
1047         msg.addr = client->addr;
1048         msg.flags = client->flags;
1049         msg.buf = buf;
1050         msg.len = sizeof(buf);
1051
1052         ret = i2c_transfer(client->adapter, &msg, 1);
1053         if (ret < 0) {
1054                 dev_err(&client->dev, "%s: error: reg=%x, val=%x\n",
1055                         __func__, reg, val);
1056                 return ret;
1057         }
1058
1059         return 0;
1060 }
1061
1062 static int ov5640_read_reg(struct ov5640_dev *sensor, u16 reg, u8 *val)
1063 {
1064         struct i2c_client *client = sensor->i2c_client;
1065         struct i2c_msg msg[2];
1066         u8 buf[2];
1067         int ret;
1068
1069         buf[0] = reg >> 8;
1070         buf[1] = reg & 0xff;
1071
1072         msg[0].addr = client->addr;
1073         msg[0].flags = client->flags;
1074         msg[0].buf = buf;
1075         msg[0].len = sizeof(buf);
1076
1077         msg[1].addr = client->addr;
1078         msg[1].flags = client->flags | I2C_M_RD;
1079         msg[1].buf = buf;
1080         msg[1].len = 1;
1081
1082         ret = i2c_transfer(client->adapter, msg, 2);
1083         if (ret < 0) {
1084                 dev_err(&client->dev, "%s: error: reg=%x\n",
1085                         __func__, reg);
1086                 return ret;
1087         }
1088
1089         *val = buf[0];
1090         return 0;
1091 }
1092
1093 static int ov5640_read_reg16(struct ov5640_dev *sensor, u16 reg, u16 *val)
1094 {
1095         u8 hi, lo;
1096         int ret;
1097
1098         ret = ov5640_read_reg(sensor, reg, &hi);
1099         if (ret)
1100                 return ret;
1101         ret = ov5640_read_reg(sensor, reg + 1, &lo);
1102         if (ret)
1103                 return ret;
1104
1105         *val = ((u16)hi << 8) | (u16)lo;
1106         return 0;
1107 }
1108
1109 static int ov5640_write_reg16(struct ov5640_dev *sensor, u16 reg, u16 val)
1110 {
1111         int ret;
1112
1113         ret = ov5640_write_reg(sensor, reg, val >> 8);
1114         if (ret)
1115                 return ret;
1116
1117         return ov5640_write_reg(sensor, reg + 1, val & 0xff);
1118 }
1119
1120 static int ov5640_mod_reg(struct ov5640_dev *sensor, u16 reg,
1121                           u8 mask, u8 val)
1122 {
1123         u8 readval;
1124         int ret;
1125
1126         ret = ov5640_read_reg(sensor, reg, &readval);
1127         if (ret)
1128                 return ret;
1129
1130         readval &= ~mask;
1131         val &= mask;
1132         val |= readval;
1133
1134         return ov5640_write_reg(sensor, reg, val);
1135 }
1136
1137 /*
1138  * After trying the various combinations, reading various
1139  * documentations spread around the net, and from the various
1140  * feedback, the clock tree is probably as follows:
1141  *
1142  *   +--------------+
1143  *   |  Ext. Clock  |
1144  *   +-+------------+
1145  *     |  +----------+
1146  *     +->|   PLL1   | - reg 0x3036, for the multiplier
1147  *        +-+--------+ - reg 0x3037, bits 0-3 for the pre-divider
1148  *          |  +--------------+
1149  *          +->| System Clock |  - reg 0x3035, bits 4-7
1150  *             +-+------------+
1151  *               |  +--------------+
1152  *               +->| MIPI Divider | - reg 0x3035, bits 0-3
1153  *               |  +-+------------+
1154  *               |    +----------------> MIPI SCLK
1155  *               |    +  +-----+
1156  *               |    +->| / 2 |-------> MIPI BIT CLK
1157  *               |       +-----+
1158  *               |  +--------------+
1159  *               +->| PLL Root Div | - reg 0x3037, bit 4
1160  *                  +-+------------+
1161  *                    |  +---------+
1162  *                    +->| Bit Div | - reg 0x3034, bits 0-3
1163  *                       +-+-------+
1164  *                         |  +-------------+
1165  *                         +->| SCLK Div    | - reg 0x3108, bits 0-1
1166  *                         |  +-+-----------+
1167  *                         |    +---------------> SCLK
1168  *                         |  +-------------+
1169  *                         +->| SCLK 2X Div | - reg 0x3108, bits 2-3
1170  *                         |  +-+-----------+
1171  *                         |    +---------------> SCLK 2X
1172  *                         |  +-------------+
1173  *                         +->| PCLK Div    | - reg 0x3108, bits 4-5
1174  *                            ++------------+
1175  *                             +  +-----------+
1176  *                             +->|   P_DIV   | - reg 0x3035, bits 0-3
1177  *                                +-----+-----+
1178  *                                       +------------> PCLK
1179  *
1180  * There seems to be also constraints:
1181  *  - the PLL pre-divider output rate should be in the 4-27MHz range
1182  *  - the PLL multiplier output rate should be in the 500-1000MHz range
1183  *  - PCLK >= SCLK * 2 in YUV, >= SCLK in Raw or JPEG
1184  */
1185
1186 /*
1187  * This is supposed to be ranging from 1 to 8, but the value is always
1188  * set to 3 in the vendor kernels.
1189  */
1190 #define OV5640_PLL_PREDIV       3
1191
1192 #define OV5640_PLL_MULT_MIN     4
1193 #define OV5640_PLL_MULT_MAX     252
1194
1195 /*
1196  * This is supposed to be ranging from 1 to 16, but the value is
1197  * always set to either 1 or 2 in the vendor kernels.
1198  */
1199 #define OV5640_SYSDIV_MIN       1
1200 #define OV5640_SYSDIV_MAX       16
1201
1202 /*
1203  * This is supposed to be ranging from 1 to 2, but the value is always
1204  * set to 2 in the vendor kernels.
1205  */
1206 #define OV5640_PLL_ROOT_DIV                     2
1207 #define OV5640_PLL_CTRL3_PLL_ROOT_DIV_2         BIT(4)
1208
1209 /*
1210  * We only supports 8-bit formats at the moment
1211  */
1212 #define OV5640_BIT_DIV                          2
1213 #define OV5640_PLL_CTRL0_MIPI_MODE_8BIT         0x08
1214
1215 /*
1216  * This is supposed to be ranging from 1 to 8, but the value is always
1217  * set to 2 in the vendor kernels.
1218  */
1219 #define OV5640_SCLK_ROOT_DIV    2
1220
1221 /*
1222  * This is hardcoded so that the consistency is maintained between SCLK and
1223  * SCLK 2x.
1224  */
1225 #define OV5640_SCLK2X_ROOT_DIV (OV5640_SCLK_ROOT_DIV / 2)
1226
1227 /*
1228  * This is supposed to be ranging from 1 to 8, but the value is always
1229  * set to 1 in the vendor kernels.
1230  */
1231 #define OV5640_PCLK_ROOT_DIV                    1
1232 #define OV5640_PLL_SYS_ROOT_DIVIDER_BYPASS      0x00
1233
1234 static unsigned long ov5640_compute_sys_clk(struct ov5640_dev *sensor,
1235                                             u8 pll_prediv, u8 pll_mult,
1236                                             u8 sysdiv)
1237 {
1238         unsigned long sysclk = sensor->xclk_freq / pll_prediv * pll_mult;
1239
1240         /* PLL1 output cannot exceed 1GHz. */
1241         if (sysclk / 1000000 > 1000)
1242                 return 0;
1243
1244         return sysclk / sysdiv;
1245 }
1246
1247 static unsigned long ov5640_calc_sys_clk(struct ov5640_dev *sensor,
1248                                          unsigned long rate,
1249                                          u8 *pll_prediv, u8 *pll_mult,
1250                                          u8 *sysdiv)
1251 {
1252         unsigned long best = ~0;
1253         u8 best_sysdiv = 1, best_mult = 1;
1254         u8 _sysdiv, _pll_mult;
1255
1256         for (_sysdiv = OV5640_SYSDIV_MIN;
1257              _sysdiv <= OV5640_SYSDIV_MAX;
1258              _sysdiv++) {
1259                 for (_pll_mult = OV5640_PLL_MULT_MIN;
1260                      _pll_mult <= OV5640_PLL_MULT_MAX;
1261                      _pll_mult++) {
1262                         unsigned long _rate;
1263
1264                         /*
1265                          * The PLL multiplier cannot be odd if above
1266                          * 127.
1267                          */
1268                         if (_pll_mult > 127 && (_pll_mult % 2))
1269                                 continue;
1270
1271                         _rate = ov5640_compute_sys_clk(sensor,
1272                                                        OV5640_PLL_PREDIV,
1273                                                        _pll_mult, _sysdiv);
1274
1275                         /*
1276                          * We have reached the maximum allowed PLL1 output,
1277                          * increase sysdiv.
1278                          */
1279                         if (!_rate)
1280                                 break;
1281
1282                         /*
1283                          * Prefer rates above the expected clock rate than
1284                          * below, even if that means being less precise.
1285                          */
1286                         if (_rate < rate)
1287                                 continue;
1288
1289                         if (abs(rate - _rate) < abs(rate - best)) {
1290                                 best = _rate;
1291                                 best_sysdiv = _sysdiv;
1292                                 best_mult = _pll_mult;
1293                         }
1294
1295                         if (_rate == rate)
1296                                 goto out;
1297                 }
1298         }
1299
1300 out:
1301         *sysdiv = best_sysdiv;
1302         *pll_prediv = OV5640_PLL_PREDIV;
1303         *pll_mult = best_mult;
1304
1305         return best;
1306 }
1307
1308 /*
1309  * ov5640_set_mipi_pclk() - Calculate the clock tree configuration values
1310  *                          for the MIPI CSI-2 output.
1311  */
1312 static int ov5640_set_mipi_pclk(struct ov5640_dev *sensor)
1313 {
1314         u8 bit_div, mipi_div, pclk_div, sclk_div, sclk2x_div, root_div;
1315         u8 prediv, mult, sysdiv;
1316         unsigned long link_freq;
1317         unsigned long sysclk;
1318         u8 pclk_period;
1319         u32 sample_rate;
1320         u32 num_lanes;
1321         int ret;
1322
1323         /* Use the link freq computed at ov5640_update_pixel_rate() time. */
1324         link_freq = sensor->current_link_freq;
1325
1326         /*
1327          * - mipi_div - Additional divider for the MIPI lane clock.
1328          *
1329          * Higher link frequencies would make sysclk > 1GHz.
1330          * Keep the sysclk low and do not divide in the MIPI domain.
1331          */
1332         if (link_freq > OV5640_LINK_RATE_MAX)
1333                 mipi_div = 1;
1334         else
1335                 mipi_div = 2;
1336
1337         sysclk = link_freq * mipi_div;
1338         ov5640_calc_sys_clk(sensor, sysclk, &prediv, &mult, &sysdiv);
1339
1340         /*
1341          * Adjust PLL parameters to maintain the MIPI_SCLK-to-PCLK ratio.
1342          *
1343          * - root_div = 2 (fixed)
1344          * - bit_div : MIPI 8-bit = 2; MIPI 10-bit = 2.5
1345          * - pclk_div = 1 (fixed)
1346          * - p_div  = (2 lanes ? mipi_div : 2 * mipi_div)
1347          *
1348          * This results in the following MIPI_SCLK depending on the number
1349          * of lanes:
1350          *
1351          * - 2 lanes: MIPI_SCLK = (4 or 5) * PCLK
1352          * - 1 lanes: MIPI_SCLK = (8 or 10) * PCLK
1353          */
1354         root_div = OV5640_PLL_CTRL3_PLL_ROOT_DIV_2;
1355         bit_div =  OV5640_PLL_CTRL0_MIPI_MODE_8BIT;
1356         pclk_div = ilog2(OV5640_PCLK_ROOT_DIV);
1357
1358         /*
1359          * Scaler clock:
1360          * - YUV: PCLK >= 2 * SCLK
1361          * - RAW or JPEG: PCLK >= SCLK
1362          * - sclk2x_div = sclk_div / 2
1363          */
1364         sclk_div = ilog2(OV5640_SCLK_ROOT_DIV);
1365         sclk2x_div = ilog2(OV5640_SCLK2X_ROOT_DIV);
1366
1367         /*
1368          * Set the pixel clock period expressed in ns with 1-bit decimal
1369          * (0x01=0.5ns).
1370          *
1371          * The register is very briefly documented. In the OV5645 datasheet it
1372          * is described as (2 * pclk period), and from testing it seems the
1373          * actual definition is 2 * 8-bit sample period.
1374          *
1375          * 2 * sample_period = (mipi_clk * 2 * num_lanes / bpp) * (bpp / 8) / 2
1376          */
1377         num_lanes = sensor->ep.bus.mipi_csi2.num_data_lanes;
1378         sample_rate = (link_freq * mipi_div * num_lanes * 2) / 16;
1379         pclk_period = 2000000000UL / sample_rate;
1380
1381         /* Program the clock tree registers. */
1382         ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL0, 0x0f, bit_div);
1383         if (ret)
1384                 return ret;
1385
1386         ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL1, 0xff,
1387                              (sysdiv << 4) | mipi_div);
1388         if (ret)
1389                 return ret;
1390
1391         ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL2, 0xff, mult);
1392         if (ret)
1393                 return ret;
1394
1395         ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL3, 0x1f,
1396                              root_div | prediv);
1397         if (ret)
1398                 return ret;
1399
1400         ret = ov5640_mod_reg(sensor, OV5640_REG_SYS_ROOT_DIVIDER, 0x3f,
1401                              (pclk_div << 4) | (sclk2x_div << 2) | sclk_div);
1402         if (ret)
1403                 return ret;
1404
1405         return ov5640_write_reg(sensor, OV5640_REG_PCLK_PERIOD, pclk_period);
1406 }
1407
1408 static u32 ov5640_calc_pixel_rate(struct ov5640_dev *sensor)
1409 {
1410         const struct ov5640_mode_info *mode = sensor->current_mode;
1411         const struct ov5640_timings *timings = &mode->dvp_timings;
1412         u32 rate;
1413
1414         rate = timings->htot * (timings->crop.height + timings->vblank_def);
1415         rate *= ov5640_framerates[sensor->current_fr];
1416
1417         return rate;
1418 }
1419
1420 static unsigned long ov5640_calc_pclk(struct ov5640_dev *sensor,
1421                                       unsigned long rate,
1422                                       u8 *pll_prediv, u8 *pll_mult, u8 *sysdiv,
1423                                       u8 *pll_rdiv, u8 *bit_div, u8 *pclk_div)
1424 {
1425         unsigned long _rate = rate * OV5640_PLL_ROOT_DIV * OV5640_BIT_DIV *
1426                                 OV5640_PCLK_ROOT_DIV;
1427
1428         _rate = ov5640_calc_sys_clk(sensor, _rate, pll_prediv, pll_mult,
1429                                     sysdiv);
1430         *pll_rdiv = OV5640_PLL_ROOT_DIV;
1431         *bit_div = OV5640_BIT_DIV;
1432         *pclk_div = OV5640_PCLK_ROOT_DIV;
1433
1434         return _rate / *pll_rdiv / *bit_div / *pclk_div;
1435 }
1436
1437 static int ov5640_set_dvp_pclk(struct ov5640_dev *sensor)
1438 {
1439         u8 prediv, mult, sysdiv, pll_rdiv, bit_div, pclk_div;
1440         u32 rate;
1441         int ret;
1442
1443         rate = ov5640_calc_pixel_rate(sensor);
1444         rate *= ov5640_code_to_bpp(sensor->fmt.code);
1445         rate /= sensor->ep.bus.parallel.bus_width;
1446
1447         ov5640_calc_pclk(sensor, rate, &prediv, &mult, &sysdiv, &pll_rdiv,
1448                          &bit_div, &pclk_div);
1449
1450         if (bit_div == 2)
1451                 bit_div = 8;
1452
1453         ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL0,
1454                              0x0f, bit_div);
1455         if (ret)
1456                 return ret;
1457
1458         /*
1459          * We need to set sysdiv according to the clock, and to clear
1460          * the MIPI divider.
1461          */
1462         ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL1,
1463                              0xff, sysdiv << 4);
1464         if (ret)
1465                 return ret;
1466
1467         ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL2,
1468                              0xff, mult);
1469         if (ret)
1470                 return ret;
1471
1472         ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL3,
1473                              0x1f, prediv | ((pll_rdiv - 1) << 4));
1474         if (ret)
1475                 return ret;
1476
1477         return ov5640_mod_reg(sensor, OV5640_REG_SYS_ROOT_DIVIDER, 0x30,
1478                               (ilog2(pclk_div) << 4));
1479 }
1480
1481 /* set JPEG framing sizes */
1482 static int ov5640_set_jpeg_timings(struct ov5640_dev *sensor,
1483                                    const struct ov5640_mode_info *mode)
1484 {
1485         int ret;
1486
1487         /*
1488          * compression mode 3 timing
1489          *
1490          * Data is transmitted with programmable width (VFIFO_HSIZE).
1491          * No padding done. Last line may have less data. Varying
1492          * number of lines per frame, depending on amount of data.
1493          */
1494         ret = ov5640_mod_reg(sensor, OV5640_REG_JPG_MODE_SELECT, 0x7, 0x3);
1495         if (ret < 0)
1496                 return ret;
1497
1498         ret = ov5640_write_reg16(sensor, OV5640_REG_VFIFO_HSIZE, mode->width);
1499         if (ret < 0)
1500                 return ret;
1501
1502         return ov5640_write_reg16(sensor, OV5640_REG_VFIFO_VSIZE, mode->height);
1503 }
1504
1505 /* download ov5640 settings to sensor through i2c */
1506 static int ov5640_set_timings(struct ov5640_dev *sensor,
1507                               const struct ov5640_mode_info *mode)
1508 {
1509         const struct ov5640_timings *timings;
1510         const struct v4l2_rect *analog_crop;
1511         const struct v4l2_rect *crop;
1512         int ret;
1513
1514         if (sensor->fmt.code == MEDIA_BUS_FMT_JPEG_1X8) {
1515                 ret = ov5640_set_jpeg_timings(sensor, mode);
1516                 if (ret < 0)
1517                         return ret;
1518         }
1519
1520         timings = ov5640_timings(sensor, mode);
1521         analog_crop = &timings->analog_crop;
1522         crop = &timings->crop;
1523
1524         ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_HS,
1525                                  analog_crop->left);
1526         if (ret < 0)
1527                 return ret;
1528
1529         ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_VS,
1530                                  analog_crop->top);
1531         if (ret < 0)
1532                 return ret;
1533
1534         ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_HW,
1535                                  analog_crop->left + analog_crop->width - 1);
1536         if (ret < 0)
1537                 return ret;
1538
1539         ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_VH,
1540                                  analog_crop->top + analog_crop->height - 1);
1541         if (ret < 0)
1542                 return ret;
1543
1544         ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_HOFFS, crop->left);
1545         if (ret < 0)
1546                 return ret;
1547
1548         ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_VOFFS, crop->top);
1549         if (ret < 0)
1550                 return ret;
1551
1552         ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_DVPHO, mode->width);
1553         if (ret < 0)
1554                 return ret;
1555
1556         ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_DVPVO, mode->height);
1557         if (ret < 0)
1558                 return ret;
1559
1560         ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_HTS, timings->htot);
1561         if (ret < 0)
1562                 return ret;
1563
1564         ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_VTS,
1565                                  mode->height + timings->vblank_def);
1566         if (ret < 0)
1567                 return ret;
1568
1569         return 0;
1570 }
1571
1572 static void ov5640_load_regs(struct ov5640_dev *sensor,
1573                              const struct reg_value *regs, unsigned int regnum)
1574 {
1575         unsigned int i;
1576         u32 delay_ms;
1577         u16 reg_addr;
1578         u8 mask, val;
1579         int ret = 0;
1580
1581         for (i = 0; i < regnum; ++i, ++regs) {
1582                 delay_ms = regs->delay_ms;
1583                 reg_addr = regs->reg_addr;
1584                 val = regs->val;
1585                 mask = regs->mask;
1586
1587                 /* remain in power down mode for DVP */
1588                 if (regs->reg_addr == OV5640_REG_SYS_CTRL0 &&
1589                     val == OV5640_REG_SYS_CTRL0_SW_PWUP &&
1590                     !ov5640_is_csi2(sensor))
1591                         continue;
1592
1593                 if (mask)
1594                         ret = ov5640_mod_reg(sensor, reg_addr, mask, val);
1595                 else
1596                         ret = ov5640_write_reg(sensor, reg_addr, val);
1597                 if (ret)
1598                         break;
1599
1600                 if (delay_ms)
1601                         usleep_range(1000 * delay_ms, 1000 * delay_ms + 100);
1602         }
1603 }
1604
1605 static int ov5640_set_autoexposure(struct ov5640_dev *sensor, bool on)
1606 {
1607         return ov5640_mod_reg(sensor, OV5640_REG_AEC_PK_MANUAL,
1608                               BIT(0), on ? 0 : BIT(0));
1609 }
1610
1611 /* read exposure, in number of line periods */
1612 static int ov5640_get_exposure(struct ov5640_dev *sensor)
1613 {
1614         int exp, ret;
1615         u8 temp;
1616
1617         ret = ov5640_read_reg(sensor, OV5640_REG_AEC_PK_EXPOSURE_HI, &temp);
1618         if (ret)
1619                 return ret;
1620         exp = ((int)temp & 0x0f) << 16;
1621         ret = ov5640_read_reg(sensor, OV5640_REG_AEC_PK_EXPOSURE_MED, &temp);
1622         if (ret)
1623                 return ret;
1624         exp |= ((int)temp << 8);
1625         ret = ov5640_read_reg(sensor, OV5640_REG_AEC_PK_EXPOSURE_LO, &temp);
1626         if (ret)
1627                 return ret;
1628         exp |= (int)temp;
1629
1630         return exp >> 4;
1631 }
1632
1633 /* write exposure, given number of line periods */
1634 static int ov5640_set_exposure(struct ov5640_dev *sensor, u32 exposure)
1635 {
1636         int ret;
1637
1638         exposure <<= 4;
1639
1640         ret = ov5640_write_reg(sensor,
1641                                OV5640_REG_AEC_PK_EXPOSURE_LO,
1642                                exposure & 0xff);
1643         if (ret)
1644                 return ret;
1645         ret = ov5640_write_reg(sensor,
1646                                OV5640_REG_AEC_PK_EXPOSURE_MED,
1647                                (exposure >> 8) & 0xff);
1648         if (ret)
1649                 return ret;
1650         return ov5640_write_reg(sensor,
1651                                 OV5640_REG_AEC_PK_EXPOSURE_HI,
1652                                 (exposure >> 16) & 0x0f);
1653 }
1654
1655 static int ov5640_get_gain(struct ov5640_dev *sensor)
1656 {
1657         u16 gain;
1658         int ret;
1659
1660         ret = ov5640_read_reg16(sensor, OV5640_REG_AEC_PK_REAL_GAIN, &gain);
1661         if (ret)
1662                 return ret;
1663
1664         return gain & 0x3ff;
1665 }
1666
1667 static int ov5640_set_gain(struct ov5640_dev *sensor, int gain)
1668 {
1669         return ov5640_write_reg16(sensor, OV5640_REG_AEC_PK_REAL_GAIN,
1670                                   (u16)gain & 0x3ff);
1671 }
1672
1673 static int ov5640_set_autogain(struct ov5640_dev *sensor, bool on)
1674 {
1675         return ov5640_mod_reg(sensor, OV5640_REG_AEC_PK_MANUAL,
1676                               BIT(1), on ? 0 : BIT(1));
1677 }
1678
1679 static int ov5640_set_stream_dvp(struct ov5640_dev *sensor, bool on)
1680 {
1681         return ov5640_write_reg(sensor, OV5640_REG_SYS_CTRL0, on ?
1682                                 OV5640_REG_SYS_CTRL0_SW_PWUP :
1683                                 OV5640_REG_SYS_CTRL0_SW_PWDN);
1684 }
1685
1686 static int ov5640_set_stream_mipi(struct ov5640_dev *sensor, bool on)
1687 {
1688         int ret;
1689
1690         /*
1691          * Enable/disable the MIPI interface
1692          *
1693          * 0x300e = on ? 0x45 : 0x40
1694          *
1695          * FIXME: the sensor manual (version 2.03) reports
1696          * [7:5] = 000  : 1 data lane mode
1697          * [7:5] = 001  : 2 data lanes mode
1698          * But this settings do not work, while the following ones
1699          * have been validated for 2 data lanes mode.
1700          *
1701          * [7:5] = 010  : 2 data lanes mode
1702          * [4] = 0      : Power up MIPI HS Tx
1703          * [3] = 0      : Power up MIPI LS Rx
1704          * [2] = 1/0    : MIPI interface enable/disable
1705          * [1:0] = 01/00: FIXME: 'debug'
1706          */
1707         ret = ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00,
1708                                on ? 0x45 : 0x40);
1709         if (ret)
1710                 return ret;
1711
1712         return ov5640_write_reg(sensor, OV5640_REG_FRAME_CTRL01,
1713                                 on ? 0x00 : 0x0f);
1714 }
1715
1716 static int ov5640_get_sysclk(struct ov5640_dev *sensor)
1717 {
1718          /* calculate sysclk */
1719         u32 xvclk = sensor->xclk_freq / 10000;
1720         u32 multiplier, prediv, VCO, sysdiv, pll_rdiv;
1721         u32 sclk_rdiv_map[] = {1, 2, 4, 8};
1722         u32 bit_div2x = 1, sclk_rdiv, sysclk;
1723         u8 temp1, temp2;
1724         int ret;
1725
1726         ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL0, &temp1);
1727         if (ret)
1728                 return ret;
1729         temp2 = temp1 & 0x0f;
1730         if (temp2 == 8 || temp2 == 10)
1731                 bit_div2x = temp2 / 2;
1732
1733         ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL1, &temp1);
1734         if (ret)
1735                 return ret;
1736         sysdiv = temp1 >> 4;
1737         if (sysdiv == 0)
1738                 sysdiv = 16;
1739
1740         ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL2, &temp1);
1741         if (ret)
1742                 return ret;
1743         multiplier = temp1;
1744
1745         ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL3, &temp1);
1746         if (ret)
1747                 return ret;
1748         prediv = temp1 & 0x0f;
1749         pll_rdiv = ((temp1 >> 4) & 0x01) + 1;
1750
1751         ret = ov5640_read_reg(sensor, OV5640_REG_SYS_ROOT_DIVIDER, &temp1);
1752         if (ret)
1753                 return ret;
1754         temp2 = temp1 & 0x03;
1755         sclk_rdiv = sclk_rdiv_map[temp2];
1756
1757         if (!prediv || !sysdiv || !pll_rdiv || !bit_div2x)
1758                 return -EINVAL;
1759
1760         VCO = xvclk * multiplier / prediv;
1761
1762         sysclk = VCO / sysdiv / pll_rdiv * 2 / bit_div2x / sclk_rdiv;
1763
1764         return sysclk;
1765 }
1766
1767 static int ov5640_set_night_mode(struct ov5640_dev *sensor)
1768 {
1769          /* read HTS from register settings */
1770         u8 mode;
1771         int ret;
1772
1773         ret = ov5640_read_reg(sensor, OV5640_REG_AEC_CTRL00, &mode);
1774         if (ret)
1775                 return ret;
1776         mode &= 0xfb;
1777         return ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL00, mode);
1778 }
1779
1780 static int ov5640_get_hts(struct ov5640_dev *sensor)
1781 {
1782         /* read HTS from register settings */
1783         u16 hts;
1784         int ret;
1785
1786         ret = ov5640_read_reg16(sensor, OV5640_REG_TIMING_HTS, &hts);
1787         if (ret)
1788                 return ret;
1789         return hts;
1790 }
1791
1792 static int ov5640_get_vts(struct ov5640_dev *sensor)
1793 {
1794         u16 vts;
1795         int ret;
1796
1797         ret = ov5640_read_reg16(sensor, OV5640_REG_TIMING_VTS, &vts);
1798         if (ret)
1799                 return ret;
1800         return vts;
1801 }
1802
1803 static int ov5640_set_vts(struct ov5640_dev *sensor, int vts)
1804 {
1805         return ov5640_write_reg16(sensor, OV5640_REG_TIMING_VTS, vts);
1806 }
1807
1808 static int ov5640_get_light_freq(struct ov5640_dev *sensor)
1809 {
1810         /* get banding filter value */
1811         int ret, light_freq = 0;
1812         u8 temp, temp1;
1813
1814         ret = ov5640_read_reg(sensor, OV5640_REG_HZ5060_CTRL01, &temp);
1815         if (ret)
1816                 return ret;
1817
1818         if (temp & 0x80) {
1819                 /* manual */
1820                 ret = ov5640_read_reg(sensor, OV5640_REG_HZ5060_CTRL00,
1821                                       &temp1);
1822                 if (ret)
1823                         return ret;
1824                 if (temp1 & 0x04) {
1825                         /* 50Hz */
1826                         light_freq = 50;
1827                 } else {
1828                         /* 60Hz */
1829                         light_freq = 60;
1830                 }
1831         } else {
1832                 /* auto */
1833                 ret = ov5640_read_reg(sensor, OV5640_REG_SIGMADELTA_CTRL0C,
1834                                       &temp1);
1835                 if (ret)
1836                         return ret;
1837
1838                 if (temp1 & 0x01) {
1839                         /* 50Hz */
1840                         light_freq = 50;
1841                 } else {
1842                         /* 60Hz */
1843                 }
1844         }
1845
1846         return light_freq;
1847 }
1848
1849 static int ov5640_set_bandingfilter(struct ov5640_dev *sensor)
1850 {
1851         u32 band_step60, max_band60, band_step50, max_band50, prev_vts;
1852         int ret;
1853
1854         /* read preview PCLK */
1855         ret = ov5640_get_sysclk(sensor);
1856         if (ret < 0)
1857                 return ret;
1858         if (ret == 0)
1859                 return -EINVAL;
1860         sensor->prev_sysclk = ret;
1861         /* read preview HTS */
1862         ret = ov5640_get_hts(sensor);
1863         if (ret < 0)
1864                 return ret;
1865         if (ret == 0)
1866                 return -EINVAL;
1867         sensor->prev_hts = ret;
1868
1869         /* read preview VTS */
1870         ret = ov5640_get_vts(sensor);
1871         if (ret < 0)
1872                 return ret;
1873         prev_vts = ret;
1874
1875         /* calculate banding filter */
1876         /* 60Hz */
1877         band_step60 = sensor->prev_sysclk * 100 / sensor->prev_hts * 100 / 120;
1878         ret = ov5640_write_reg16(sensor, OV5640_REG_AEC_B60_STEP, band_step60);
1879         if (ret)
1880                 return ret;
1881         if (!band_step60)
1882                 return -EINVAL;
1883         max_band60 = (int)((prev_vts - 4) / band_step60);
1884         ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL0D, max_band60);
1885         if (ret)
1886                 return ret;
1887
1888         /* 50Hz */
1889         band_step50 = sensor->prev_sysclk * 100 / sensor->prev_hts;
1890         ret = ov5640_write_reg16(sensor, OV5640_REG_AEC_B50_STEP, band_step50);
1891         if (ret)
1892                 return ret;
1893         if (!band_step50)
1894                 return -EINVAL;
1895         max_band50 = (int)((prev_vts - 4) / band_step50);
1896         return ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL0E, max_band50);
1897 }
1898
1899 static int ov5640_set_ae_target(struct ov5640_dev *sensor, int target)
1900 {
1901         /* stable in high */
1902         u32 fast_high, fast_low;
1903         int ret;
1904
1905         sensor->ae_low = target * 23 / 25;      /* 0.92 */
1906         sensor->ae_high = target * 27 / 25;     /* 1.08 */
1907
1908         fast_high = sensor->ae_high << 1;
1909         if (fast_high > 255)
1910                 fast_high = 255;
1911
1912         fast_low = sensor->ae_low >> 1;
1913
1914         ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL0F, sensor->ae_high);
1915         if (ret)
1916                 return ret;
1917         ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL10, sensor->ae_low);
1918         if (ret)
1919                 return ret;
1920         ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL1B, sensor->ae_high);
1921         if (ret)
1922                 return ret;
1923         ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL1E, sensor->ae_low);
1924         if (ret)
1925                 return ret;
1926         ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL11, fast_high);
1927         if (ret)
1928                 return ret;
1929         return ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL1F, fast_low);
1930 }
1931
1932 static int ov5640_get_binning(struct ov5640_dev *sensor)
1933 {
1934         u8 temp;
1935         int ret;
1936
1937         ret = ov5640_read_reg(sensor, OV5640_REG_TIMING_TC_REG21, &temp);
1938         if (ret)
1939                 return ret;
1940
1941         return temp & BIT(0);
1942 }
1943
1944 static int ov5640_set_binning(struct ov5640_dev *sensor, bool enable)
1945 {
1946         int ret;
1947
1948         /*
1949          * TIMING TC REG21:
1950          * - [0]:       Horizontal binning enable
1951          */
1952         ret = ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG21,
1953                              BIT(0), enable ? BIT(0) : 0);
1954         if (ret)
1955                 return ret;
1956         /*
1957          * TIMING TC REG20:
1958          * - [0]:       Undocumented, but hardcoded init sequences
1959          *              are always setting REG21/REG20 bit 0 to same value...
1960          */
1961         return ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG20,
1962                               BIT(0), enable ? BIT(0) : 0);
1963 }
1964
1965 static int ov5640_set_virtual_channel(struct ov5640_dev *sensor)
1966 {
1967         struct i2c_client *client = sensor->i2c_client;
1968         u8 temp, channel = virtual_channel;
1969         int ret;
1970
1971         if (channel > 3) {
1972                 dev_err(&client->dev,
1973                         "%s: wrong virtual_channel parameter, expected (0..3), got %d\n",
1974                         __func__, channel);
1975                 return -EINVAL;
1976         }
1977
1978         ret = ov5640_read_reg(sensor, OV5640_REG_DEBUG_MODE, &temp);
1979         if (ret)
1980                 return ret;
1981         temp &= ~(3 << 6);
1982         temp |= (channel << 6);
1983         return ov5640_write_reg(sensor, OV5640_REG_DEBUG_MODE, temp);
1984 }
1985
1986 static const struct ov5640_mode_info *
1987 ov5640_find_mode(struct ov5640_dev *sensor, enum ov5640_frame_rate fr,
1988                  int width, int height, bool nearest)
1989 {
1990         const struct ov5640_mode_info *mode;
1991
1992         mode = v4l2_find_nearest_size(ov5640_mode_data,
1993                                       ARRAY_SIZE(ov5640_mode_data),
1994                                       width, height, width, height);
1995
1996         if (!mode ||
1997             (!nearest &&
1998              (mode->width != width || mode->height != height)))
1999                 return NULL;
2000
2001         /* Check to see if the current mode exceeds the max frame rate */
2002         if (ov5640_framerates[fr] > ov5640_framerates[mode->max_fps])
2003                 return NULL;
2004
2005         return mode;
2006 }
2007
2008 /*
2009  * sensor changes between scaling and subsampling, go through
2010  * exposure calculation
2011  */
2012 static int ov5640_set_mode_exposure_calc(struct ov5640_dev *sensor,
2013                                          const struct ov5640_mode_info *mode)
2014 {
2015         u32 prev_shutter, prev_gain16;
2016         u32 cap_shutter, cap_gain16;
2017         u32 cap_sysclk, cap_hts, cap_vts;
2018         u32 light_freq, cap_bandfilt, cap_maxband;
2019         u32 cap_gain16_shutter;
2020         u8 average;
2021         int ret;
2022
2023         if (!mode->reg_data)
2024                 return -EINVAL;
2025
2026         /* read preview shutter */
2027         ret = ov5640_get_exposure(sensor);
2028         if (ret < 0)
2029                 return ret;
2030         prev_shutter = ret;
2031         ret = ov5640_get_binning(sensor);
2032         if (ret < 0)
2033                 return ret;
2034         if (ret && mode->id != OV5640_MODE_720P_1280_720 &&
2035             mode->id != OV5640_MODE_1080P_1920_1080)
2036                 prev_shutter *= 2;
2037
2038         /* read preview gain */
2039         ret = ov5640_get_gain(sensor);
2040         if (ret < 0)
2041                 return ret;
2042         prev_gain16 = ret;
2043
2044         /* get average */
2045         ret = ov5640_read_reg(sensor, OV5640_REG_AVG_READOUT, &average);
2046         if (ret)
2047                 return ret;
2048
2049         /* turn off night mode for capture */
2050         ret = ov5640_set_night_mode(sensor);
2051         if (ret < 0)
2052                 return ret;
2053
2054         /* Write capture setting */
2055         ov5640_load_regs(sensor, mode->reg_data, mode->reg_data_size);
2056         ret = ov5640_set_timings(sensor, mode);
2057         if (ret < 0)
2058                 return ret;
2059
2060         /* read capture VTS */
2061         ret = ov5640_get_vts(sensor);
2062         if (ret < 0)
2063                 return ret;
2064         cap_vts = ret;
2065         ret = ov5640_get_hts(sensor);
2066         if (ret < 0)
2067                 return ret;
2068         if (ret == 0)
2069                 return -EINVAL;
2070         cap_hts = ret;
2071
2072         ret = ov5640_get_sysclk(sensor);
2073         if (ret < 0)
2074                 return ret;
2075         if (ret == 0)
2076                 return -EINVAL;
2077         cap_sysclk = ret;
2078
2079         /* calculate capture banding filter */
2080         ret = ov5640_get_light_freq(sensor);
2081         if (ret < 0)
2082                 return ret;
2083         light_freq = ret;
2084
2085         if (light_freq == 60) {
2086                 /* 60Hz */
2087                 cap_bandfilt = cap_sysclk * 100 / cap_hts * 100 / 120;
2088         } else {
2089                 /* 50Hz */
2090                 cap_bandfilt = cap_sysclk * 100 / cap_hts;
2091         }
2092
2093         if (!sensor->prev_sysclk) {
2094                 ret = ov5640_get_sysclk(sensor);
2095                 if (ret < 0)
2096                         return ret;
2097                 if (ret == 0)
2098                         return -EINVAL;
2099                 sensor->prev_sysclk = ret;
2100         }
2101
2102         if (!cap_bandfilt)
2103                 return -EINVAL;
2104
2105         cap_maxband = (int)((cap_vts - 4) / cap_bandfilt);
2106
2107         /* calculate capture shutter/gain16 */
2108         if (average > sensor->ae_low && average < sensor->ae_high) {
2109                 /* in stable range */
2110                 cap_gain16_shutter =
2111                         prev_gain16 * prev_shutter *
2112                         cap_sysclk / sensor->prev_sysclk *
2113                         sensor->prev_hts / cap_hts *
2114                         sensor->ae_target / average;
2115         } else {
2116                 cap_gain16_shutter =
2117                         prev_gain16 * prev_shutter *
2118                         cap_sysclk / sensor->prev_sysclk *
2119                         sensor->prev_hts / cap_hts;
2120         }
2121
2122         /* gain to shutter */
2123         if (cap_gain16_shutter < (cap_bandfilt * 16)) {
2124                 /* shutter < 1/100 */
2125                 cap_shutter = cap_gain16_shutter / 16;
2126                 if (cap_shutter < 1)
2127                         cap_shutter = 1;
2128
2129                 cap_gain16 = cap_gain16_shutter / cap_shutter;
2130                 if (cap_gain16 < 16)
2131                         cap_gain16 = 16;
2132         } else {
2133                 if (cap_gain16_shutter > (cap_bandfilt * cap_maxband * 16)) {
2134                         /* exposure reach max */
2135                         cap_shutter = cap_bandfilt * cap_maxband;
2136                         if (!cap_shutter)
2137                                 return -EINVAL;
2138
2139                         cap_gain16 = cap_gain16_shutter / cap_shutter;
2140                 } else {
2141                         /* 1/100 < (cap_shutter = n/100) =< max */
2142                         cap_shutter =
2143                                 ((int)(cap_gain16_shutter / 16 / cap_bandfilt))
2144                                 * cap_bandfilt;
2145                         if (!cap_shutter)
2146                                 return -EINVAL;
2147
2148                         cap_gain16 = cap_gain16_shutter / cap_shutter;
2149                 }
2150         }
2151
2152         /* set capture gain */
2153         ret = ov5640_set_gain(sensor, cap_gain16);
2154         if (ret)
2155                 return ret;
2156
2157         /* write capture shutter */
2158         if (cap_shutter > (cap_vts - 4)) {
2159                 cap_vts = cap_shutter + 4;
2160                 ret = ov5640_set_vts(sensor, cap_vts);
2161                 if (ret < 0)
2162                         return ret;
2163         }
2164
2165         /* set exposure */
2166         return ov5640_set_exposure(sensor, cap_shutter);
2167 }
2168
2169 /*
2170  * if sensor changes inside scaling or subsampling
2171  * change mode directly
2172  */
2173 static int ov5640_set_mode_direct(struct ov5640_dev *sensor,
2174                                   const struct ov5640_mode_info *mode)
2175 {
2176         if (!mode->reg_data)
2177                 return -EINVAL;
2178
2179         /* Write capture setting */
2180         ov5640_load_regs(sensor, mode->reg_data, mode->reg_data_size);
2181         return ov5640_set_timings(sensor, mode);
2182 }
2183
2184 static int ov5640_set_mode(struct ov5640_dev *sensor)
2185 {
2186         const struct ov5640_mode_info *mode = sensor->current_mode;
2187         const struct ov5640_mode_info *orig_mode = sensor->last_mode;
2188         enum ov5640_downsize_mode dn_mode, orig_dn_mode;
2189         bool auto_gain = sensor->ctrls.auto_gain->val == 1;
2190         bool auto_exp =  sensor->ctrls.auto_exp->val == V4L2_EXPOSURE_AUTO;
2191         int ret;
2192
2193         dn_mode = mode->dn_mode;
2194         orig_dn_mode = orig_mode->dn_mode;
2195
2196         /* auto gain and exposure must be turned off when changing modes */
2197         if (auto_gain) {
2198                 ret = ov5640_set_autogain(sensor, false);
2199                 if (ret)
2200                         return ret;
2201         }
2202
2203         if (auto_exp) {
2204                 ret = ov5640_set_autoexposure(sensor, false);
2205                 if (ret)
2206                         goto restore_auto_gain;
2207         }
2208
2209         if (ov5640_is_csi2(sensor))
2210                 ret = ov5640_set_mipi_pclk(sensor);
2211         else
2212                 ret = ov5640_set_dvp_pclk(sensor);
2213         if (ret < 0)
2214                 return 0;
2215
2216         if ((dn_mode == SUBSAMPLING && orig_dn_mode == SCALING) ||
2217             (dn_mode == SCALING && orig_dn_mode == SUBSAMPLING)) {
2218                 /*
2219                  * change between subsampling and scaling
2220                  * go through exposure calculation
2221                  */
2222                 ret = ov5640_set_mode_exposure_calc(sensor, mode);
2223         } else {
2224                 /*
2225                  * change inside subsampling or scaling
2226                  * download firmware directly
2227                  */
2228                 ret = ov5640_set_mode_direct(sensor, mode);
2229         }
2230         if (ret < 0)
2231                 goto restore_auto_exp_gain;
2232
2233         /* restore auto gain and exposure */
2234         if (auto_gain)
2235                 ov5640_set_autogain(sensor, true);
2236         if (auto_exp)
2237                 ov5640_set_autoexposure(sensor, true);
2238
2239         ret = ov5640_set_binning(sensor, dn_mode != SCALING);
2240         if (ret < 0)
2241                 return ret;
2242         ret = ov5640_set_ae_target(sensor, sensor->ae_target);
2243         if (ret < 0)
2244                 return ret;
2245         ret = ov5640_get_light_freq(sensor);
2246         if (ret < 0)
2247                 return ret;
2248         ret = ov5640_set_bandingfilter(sensor);
2249         if (ret < 0)
2250                 return ret;
2251         ret = ov5640_set_virtual_channel(sensor);
2252         if (ret < 0)
2253                 return ret;
2254
2255         sensor->pending_mode_change = false;
2256         sensor->last_mode = mode;
2257
2258         return 0;
2259
2260 restore_auto_exp_gain:
2261         if (auto_exp)
2262                 ov5640_set_autoexposure(sensor, true);
2263 restore_auto_gain:
2264         if (auto_gain)
2265                 ov5640_set_autogain(sensor, true);
2266
2267         return ret;
2268 }
2269
2270 static int ov5640_set_framefmt(struct ov5640_dev *sensor,
2271                                struct v4l2_mbus_framefmt *format);
2272
2273 /* restore the last set video mode after chip power-on */
2274 static int ov5640_restore_mode(struct ov5640_dev *sensor)
2275 {
2276         int ret;
2277
2278         /* first load the initial register values */
2279         ov5640_load_regs(sensor, ov5640_init_setting,
2280                          ARRAY_SIZE(ov5640_init_setting));
2281
2282         ret = ov5640_mod_reg(sensor, OV5640_REG_SYS_ROOT_DIVIDER, 0x3f,
2283                              (ilog2(OV5640_SCLK2X_ROOT_DIV) << 2) |
2284                              ilog2(OV5640_SCLK_ROOT_DIV));
2285         if (ret)
2286                 return ret;
2287
2288         /* now restore the last capture mode */
2289         ret = ov5640_set_mode(sensor);
2290         if (ret < 0)
2291                 return ret;
2292
2293         return ov5640_set_framefmt(sensor, &sensor->fmt);
2294 }
2295
2296 static void ov5640_power(struct ov5640_dev *sensor, bool enable)
2297 {
2298         gpiod_set_value_cansleep(sensor->pwdn_gpio, enable ? 0 : 1);
2299 }
2300
2301 static void ov5640_reset(struct ov5640_dev *sensor)
2302 {
2303         if (!sensor->reset_gpio)
2304                 return;
2305
2306         gpiod_set_value_cansleep(sensor->reset_gpio, 0);
2307
2308         /* camera power cycle */
2309         ov5640_power(sensor, false);
2310         usleep_range(5000, 10000);
2311         ov5640_power(sensor, true);
2312         usleep_range(5000, 10000);
2313
2314         gpiod_set_value_cansleep(sensor->reset_gpio, 1);
2315         usleep_range(1000, 2000);
2316
2317         gpiod_set_value_cansleep(sensor->reset_gpio, 0);
2318         usleep_range(20000, 25000);
2319 }
2320
2321 static int ov5640_set_power_on(struct ov5640_dev *sensor)
2322 {
2323         struct i2c_client *client = sensor->i2c_client;
2324         int ret;
2325
2326         ret = clk_prepare_enable(sensor->xclk);
2327         if (ret) {
2328                 dev_err(&client->dev, "%s: failed to enable clock\n",
2329                         __func__);
2330                 return ret;
2331         }
2332
2333         ret = regulator_bulk_enable(OV5640_NUM_SUPPLIES,
2334                                     sensor->supplies);
2335         if (ret) {
2336                 dev_err(&client->dev, "%s: failed to enable regulators\n",
2337                         __func__);
2338                 goto xclk_off;
2339         }
2340
2341         ov5640_reset(sensor);
2342         ov5640_power(sensor, true);
2343
2344         ret = ov5640_init_slave_id(sensor);
2345         if (ret)
2346                 goto power_off;
2347
2348         return 0;
2349
2350 power_off:
2351         ov5640_power(sensor, false);
2352         regulator_bulk_disable(OV5640_NUM_SUPPLIES, sensor->supplies);
2353 xclk_off:
2354         clk_disable_unprepare(sensor->xclk);
2355         return ret;
2356 }
2357
2358 static void ov5640_set_power_off(struct ov5640_dev *sensor)
2359 {
2360         ov5640_power(sensor, false);
2361         regulator_bulk_disable(OV5640_NUM_SUPPLIES, sensor->supplies);
2362         clk_disable_unprepare(sensor->xclk);
2363 }
2364
2365 static int ov5640_set_power_mipi(struct ov5640_dev *sensor, bool on)
2366 {
2367         int ret;
2368
2369         if (!on) {
2370                 /* Reset MIPI bus settings to their default values. */
2371                 ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x58);
2372                 ov5640_write_reg(sensor, OV5640_REG_MIPI_CTRL00, 0x04);
2373                 ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT00, 0x00);
2374                 return 0;
2375         }
2376
2377         /*
2378          * Power up MIPI HS Tx and LS Rx; 2 data lanes mode
2379          *
2380          * 0x300e = 0x40
2381          * [7:5] = 010  : 2 data lanes mode (see FIXME note in
2382          *                "ov5640_set_stream_mipi()")
2383          * [4] = 0      : Power up MIPI HS Tx
2384          * [3] = 0      : Power up MIPI LS Rx
2385          * [2] = 0      : MIPI interface disabled
2386          */
2387         ret = ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x40);
2388         if (ret)
2389                 return ret;
2390
2391         /*
2392          * Gate clock and set LP11 in 'no packets mode' (idle)
2393          *
2394          * 0x4800 = 0x24
2395          * [5] = 1      : Gate clock when 'no packets'
2396          * [2] = 1      : MIPI bus in LP11 when 'no packets'
2397          */
2398         ret = ov5640_write_reg(sensor, OV5640_REG_MIPI_CTRL00, 0x24);
2399         if (ret)
2400                 return ret;
2401
2402         /*
2403          * Set data lanes and clock in LP11 when 'sleeping'
2404          *
2405          * 0x3019 = 0x70
2406          * [6] = 1      : MIPI data lane 2 in LP11 when 'sleeping'
2407          * [5] = 1      : MIPI data lane 1 in LP11 when 'sleeping'
2408          * [4] = 1      : MIPI clock lane in LP11 when 'sleeping'
2409          */
2410         ret = ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT00, 0x70);
2411         if (ret)
2412                 return ret;
2413
2414         /* Give lanes some time to coax into LP11 state. */
2415         usleep_range(500, 1000);
2416
2417         return 0;
2418 }
2419
2420 static int ov5640_set_power_dvp(struct ov5640_dev *sensor, bool on)
2421 {
2422         unsigned int flags = sensor->ep.bus.parallel.flags;
2423         bool bt656 = sensor->ep.bus_type == V4L2_MBUS_BT656;
2424         u8 polarities = 0;
2425         int ret;
2426
2427         if (!on) {
2428                 /* Reset settings to their default values. */
2429                 ov5640_write_reg(sensor, OV5640_REG_CCIR656_CTRL00, 0x00);
2430                 ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x58);
2431                 ov5640_write_reg(sensor, OV5640_REG_POLARITY_CTRL00, 0x20);
2432                 ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT_ENABLE01, 0x00);
2433                 ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT_ENABLE02, 0x00);
2434                 return 0;
2435         }
2436
2437         /*
2438          * Note about parallel port configuration.
2439          *
2440          * When configured in parallel mode, the OV5640 will
2441          * output 10 bits data on DVP data lines [9:0].
2442          * If only 8 bits data are wanted, the 8 bits data lines
2443          * of the camera interface must be physically connected
2444          * on the DVP data lines [9:2].
2445          *
2446          * Control lines polarity can be configured through
2447          * devicetree endpoint control lines properties.
2448          * If no endpoint control lines properties are set,
2449          * polarity will be as below:
2450          * - VSYNC:     active high
2451          * - HREF:      active low
2452          * - PCLK:      active low
2453          *
2454          * VSYNC & HREF are not configured if BT656 bus mode is selected
2455          */
2456
2457         /*
2458          * BT656 embedded synchronization configuration
2459          *
2460          * CCIR656 CTRL00
2461          * - [7]:       SYNC code selection (0: auto generate sync code,
2462          *              1: sync code from regs 0x4732-0x4735)
2463          * - [6]:       f value in CCIR656 SYNC code when fixed f value
2464          * - [5]:       Fixed f value
2465          * - [4:3]:     Blank toggle data options (00: data=1'h040/1'h200,
2466          *              01: data from regs 0x4736-0x4738, 10: always keep 0)
2467          * - [1]:       Clip data disable
2468          * - [0]:       CCIR656 mode enable
2469          *
2470          * Default CCIR656 SAV/EAV mode with default codes
2471          * SAV=0xff000080 & EAV=0xff00009d is enabled here with settings:
2472          * - CCIR656 mode enable
2473          * - auto generation of sync codes
2474          * - blank toggle data 1'h040/1'h200
2475          * - clip reserved data (0x00 & 0xff changed to 0x01 & 0xfe)
2476          */
2477         ret = ov5640_write_reg(sensor, OV5640_REG_CCIR656_CTRL00,
2478                                bt656 ? 0x01 : 0x00);
2479         if (ret)
2480                 return ret;
2481
2482         /*
2483          * configure parallel port control lines polarity
2484          *
2485          * POLARITY CTRL0
2486          * - [5]:       PCLK polarity (0: active low, 1: active high)
2487          * - [1]:       HREF polarity (0: active low, 1: active high)
2488          * - [0]:       VSYNC polarity (mismatch here between
2489          *              datasheet and hardware, 0 is active high
2490          *              and 1 is active low...)
2491          */
2492         if (!bt656) {
2493                 if (flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
2494                         polarities |= BIT(1);
2495                 if (flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
2496                         polarities |= BIT(0);
2497         }
2498         if (flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
2499                 polarities |= BIT(5);
2500
2501         ret = ov5640_write_reg(sensor, OV5640_REG_POLARITY_CTRL00, polarities);
2502         if (ret)
2503                 return ret;
2504
2505         /*
2506          * powerdown MIPI TX/RX PHY & enable DVP
2507          *
2508          * MIPI CONTROL 00
2509          * [4] = 1      : Power down MIPI HS Tx
2510          * [3] = 1      : Power down MIPI LS Rx
2511          * [2] = 0      : DVP enable (MIPI disable)
2512          */
2513         ret = ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x18);
2514         if (ret)
2515                 return ret;
2516
2517         /*
2518          * enable VSYNC/HREF/PCLK DVP control lines
2519          * & D[9:6] DVP data lines
2520          *
2521          * PAD OUTPUT ENABLE 01
2522          * - 6:         VSYNC output enable
2523          * - 5:         HREF output enable
2524          * - 4:         PCLK output enable
2525          * - [3:0]:     D[9:6] output enable
2526          */
2527         ret = ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT_ENABLE01,
2528                                bt656 ? 0x1f : 0x7f);
2529         if (ret)
2530                 return ret;
2531
2532         /*
2533          * enable D[5:0] DVP data lines
2534          *
2535          * PAD OUTPUT ENABLE 02
2536          * - [7:2]:     D[5:0] output enable
2537          */
2538         return ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT_ENABLE02, 0xfc);
2539 }
2540
2541 static int ov5640_set_power(struct ov5640_dev *sensor, bool on)
2542 {
2543         int ret = 0;
2544
2545         if (on) {
2546                 ret = ov5640_set_power_on(sensor);
2547                 if (ret)
2548                         return ret;
2549
2550                 ret = ov5640_restore_mode(sensor);
2551                 if (ret)
2552                         goto power_off;
2553         }
2554
2555         if (sensor->ep.bus_type == V4L2_MBUS_CSI2_DPHY)
2556                 ret = ov5640_set_power_mipi(sensor, on);
2557         else
2558                 ret = ov5640_set_power_dvp(sensor, on);
2559         if (ret)
2560                 goto power_off;
2561
2562         if (!on)
2563                 ov5640_set_power_off(sensor);
2564
2565         return 0;
2566
2567 power_off:
2568         ov5640_set_power_off(sensor);
2569         return ret;
2570 }
2571
2572 /* --------------- Subdev Operations --------------- */
2573
2574 static int ov5640_s_power(struct v4l2_subdev *sd, int on)
2575 {
2576         struct ov5640_dev *sensor = to_ov5640_dev(sd);
2577         int ret = 0;
2578
2579         mutex_lock(&sensor->lock);
2580
2581         /*
2582          * If the power count is modified from 0 to != 0 or from != 0 to 0,
2583          * update the power state.
2584          */
2585         if (sensor->power_count == !on) {
2586                 ret = ov5640_set_power(sensor, !!on);
2587                 if (ret)
2588                         goto out;
2589         }
2590
2591         /* Update the power count. */
2592         sensor->power_count += on ? 1 : -1;
2593         WARN_ON(sensor->power_count < 0);
2594 out:
2595         mutex_unlock(&sensor->lock);
2596
2597         if (on && !ret && sensor->power_count == 1) {
2598                 /* restore controls */
2599                 ret = v4l2_ctrl_handler_setup(&sensor->ctrls.handler);
2600         }
2601
2602         return ret;
2603 }
2604
2605 static int ov5640_try_frame_interval(struct ov5640_dev *sensor,
2606                                      struct v4l2_fract *fi,
2607                                      u32 width, u32 height)
2608 {
2609         const struct ov5640_mode_info *mode;
2610         enum ov5640_frame_rate rate = OV5640_15_FPS;
2611         int minfps, maxfps, best_fps, fps;
2612         int i;
2613
2614         minfps = ov5640_framerates[OV5640_15_FPS];
2615         maxfps = ov5640_framerates[OV5640_60_FPS];
2616
2617         if (fi->numerator == 0) {
2618                 fi->denominator = maxfps;
2619                 fi->numerator = 1;
2620                 rate = OV5640_60_FPS;
2621                 goto find_mode;
2622         }
2623
2624         fps = clamp_val(DIV_ROUND_CLOSEST(fi->denominator, fi->numerator),
2625                         minfps, maxfps);
2626
2627         best_fps = minfps;
2628         for (i = 0; i < ARRAY_SIZE(ov5640_framerates); i++) {
2629                 int curr_fps = ov5640_framerates[i];
2630
2631                 if (abs(curr_fps - fps) < abs(best_fps - fps)) {
2632                         best_fps = curr_fps;
2633                         rate = i;
2634                 }
2635         }
2636
2637         fi->numerator = 1;
2638         fi->denominator = best_fps;
2639
2640 find_mode:
2641         mode = ov5640_find_mode(sensor, rate, width, height, false);
2642         return mode ? rate : -EINVAL;
2643 }
2644
2645 static int ov5640_get_fmt(struct v4l2_subdev *sd,
2646                           struct v4l2_subdev_state *sd_state,
2647                           struct v4l2_subdev_format *format)
2648 {
2649         struct ov5640_dev *sensor = to_ov5640_dev(sd);
2650         struct v4l2_mbus_framefmt *fmt;
2651
2652         if (format->pad != 0)
2653                 return -EINVAL;
2654
2655         mutex_lock(&sensor->lock);
2656
2657         if (format->which == V4L2_SUBDEV_FORMAT_TRY)
2658                 fmt = v4l2_subdev_get_try_format(&sensor->sd, sd_state,
2659                                                  format->pad);
2660         else
2661                 fmt = &sensor->fmt;
2662
2663         format->format = *fmt;
2664
2665         mutex_unlock(&sensor->lock);
2666
2667         return 0;
2668 }
2669
2670 static int ov5640_try_fmt_internal(struct v4l2_subdev *sd,
2671                                    struct v4l2_mbus_framefmt *fmt,
2672                                    enum ov5640_frame_rate fr,
2673                                    const struct ov5640_mode_info **new_mode)
2674 {
2675         struct ov5640_dev *sensor = to_ov5640_dev(sd);
2676         const struct ov5640_mode_info *mode;
2677         int i;
2678
2679         mode = ov5640_find_mode(sensor, fr, fmt->width, fmt->height, true);
2680         if (!mode)
2681                 return -EINVAL;
2682         fmt->width = mode->width;
2683         fmt->height = mode->height;
2684
2685         if (new_mode)
2686                 *new_mode = mode;
2687
2688         for (i = 0; i < ARRAY_SIZE(ov5640_formats); i++)
2689                 if (ov5640_formats[i].code == fmt->code)
2690                         break;
2691         if (i >= ARRAY_SIZE(ov5640_formats))
2692                 i = 0;
2693
2694         fmt->code = ov5640_formats[i].code;
2695         fmt->colorspace = ov5640_formats[i].colorspace;
2696         fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
2697         fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
2698         fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
2699
2700         return 0;
2701 }
2702
2703 static int ov5640_update_pixel_rate(struct ov5640_dev *sensor)
2704 {
2705         const struct ov5640_mode_info *mode = sensor->current_mode;
2706         enum ov5640_pixel_rate_id pixel_rate_id = mode->pixel_rate;
2707         struct v4l2_mbus_framefmt *fmt = &sensor->fmt;
2708         const struct ov5640_timings *timings;
2709         s32 exposure_val, exposure_max;
2710         unsigned int hblank;
2711         unsigned int i = 0;
2712         u32 pixel_rate;
2713         s64 link_freq;
2714         u32 num_lanes;
2715         u32 bpp;
2716
2717         /*
2718          * Update the pixel rate control value.
2719          *
2720          * For DVP mode, maintain the pixel rate calculation using fixed FPS.
2721          */
2722         if (!ov5640_is_csi2(sensor)) {
2723                 __v4l2_ctrl_s_ctrl_int64(sensor->ctrls.pixel_rate,
2724                                          ov5640_calc_pixel_rate(sensor));
2725
2726                 return 0;
2727         }
2728
2729         /*
2730          * The MIPI CSI-2 link frequency should comply with the CSI-2
2731          * specification and be lower than 1GHz.
2732          *
2733          * Start from the suggested pixel_rate for the current mode and
2734          * progressively slow it down if it exceeds 1GHz.
2735          */
2736         num_lanes = sensor->ep.bus.mipi_csi2.num_data_lanes;
2737         bpp = ov5640_code_to_bpp(fmt->code);
2738         do {
2739                 pixel_rate = ov5640_pixel_rates[pixel_rate_id];
2740                 link_freq = pixel_rate * bpp / (2 * num_lanes);
2741         } while (link_freq >= 1000000000U &&
2742                  ++pixel_rate_id < OV5640_NUM_PIXEL_RATES);
2743
2744         sensor->current_link_freq = link_freq;
2745
2746         /*
2747          * Higher link rates require the clock tree to be programmed with
2748          * 'mipi_div' = 1; this has the effect of halving the actual output
2749          * pixel rate in the MIPI domain.
2750          *
2751          * Adjust the pixel rate and link frequency control value to report it
2752          * correctly to userspace.
2753          */
2754         if (link_freq > OV5640_LINK_RATE_MAX) {
2755                 pixel_rate /= 2;
2756                 link_freq /= 2;
2757         }
2758
2759         for (i = 0; i < ARRAY_SIZE(ov5640_csi2_link_freqs); ++i) {
2760                 if (ov5640_csi2_link_freqs[i] == link_freq)
2761                         break;
2762         }
2763         WARN_ON(i == ARRAY_SIZE(ov5640_csi2_link_freqs));
2764
2765         __v4l2_ctrl_s_ctrl_int64(sensor->ctrls.pixel_rate, pixel_rate);
2766         __v4l2_ctrl_s_ctrl(sensor->ctrls.link_freq, i);
2767
2768         timings = ov5640_timings(sensor, mode);
2769         hblank = timings->htot - mode->width;
2770         __v4l2_ctrl_modify_range(sensor->ctrls.hblank,
2771                                  hblank, hblank, 1, hblank);
2772
2773         __v4l2_ctrl_modify_range(sensor->ctrls.vblank, OV5640_MIN_VBLANK,
2774                                  OV5640_MAX_VTS - mode->height, 1,
2775                                  timings->vblank_def);
2776         __v4l2_ctrl_s_ctrl(sensor->ctrls.vblank, timings->vblank_def);
2777
2778         exposure_max = timings->crop.height + timings->vblank_def - 4;
2779         exposure_val = clamp_t(s32, sensor->ctrls.exposure->val,
2780                                sensor->ctrls.exposure->minimum,
2781                                exposure_max);
2782         __v4l2_ctrl_modify_range(sensor->ctrls.exposure,
2783                                  sensor->ctrls.exposure->minimum,
2784                                  exposure_max, 1, exposure_val);
2785
2786         return 0;
2787 }
2788
2789 static int ov5640_set_fmt(struct v4l2_subdev *sd,
2790                           struct v4l2_subdev_state *sd_state,
2791                           struct v4l2_subdev_format *format)
2792 {
2793         struct ov5640_dev *sensor = to_ov5640_dev(sd);
2794         const struct ov5640_mode_info *new_mode;
2795         struct v4l2_mbus_framefmt *mbus_fmt = &format->format;
2796         int ret;
2797
2798         if (format->pad != 0)
2799                 return -EINVAL;
2800
2801         mutex_lock(&sensor->lock);
2802
2803         if (sensor->streaming) {
2804                 ret = -EBUSY;
2805                 goto out;
2806         }
2807
2808         ret = ov5640_try_fmt_internal(sd, mbus_fmt,
2809                                       sensor->current_fr, &new_mode);
2810         if (ret)
2811                 goto out;
2812
2813         if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
2814                 *v4l2_subdev_get_try_format(sd, sd_state, 0) = *mbus_fmt;
2815                 goto out;
2816         }
2817
2818         if (new_mode != sensor->current_mode) {
2819                 sensor->current_mode = new_mode;
2820                 sensor->pending_mode_change = true;
2821         }
2822         if (mbus_fmt->code != sensor->fmt.code)
2823                 sensor->pending_fmt_change = true;
2824
2825         /* update format even if code is unchanged, resolution might change */
2826         sensor->fmt = *mbus_fmt;
2827
2828         ov5640_update_pixel_rate(sensor);
2829
2830 out:
2831         mutex_unlock(&sensor->lock);
2832         return ret;
2833 }
2834
2835 static int ov5640_set_framefmt(struct ov5640_dev *sensor,
2836                                struct v4l2_mbus_framefmt *format)
2837 {
2838         int ret = 0;
2839         bool is_jpeg = false;
2840         u8 fmt, mux;
2841
2842         switch (format->code) {
2843         case MEDIA_BUS_FMT_UYVY8_1X16:
2844         case MEDIA_BUS_FMT_UYVY8_2X8:
2845                 /* YUV422, UYVY */
2846                 fmt = 0x3f;
2847                 mux = OV5640_FMT_MUX_YUV422;
2848                 break;
2849         case MEDIA_BUS_FMT_YUYV8_1X16:
2850         case MEDIA_BUS_FMT_YUYV8_2X8:
2851                 /* YUV422, YUYV */
2852                 fmt = 0x30;
2853                 mux = OV5640_FMT_MUX_YUV422;
2854                 break;
2855         case MEDIA_BUS_FMT_RGB565_2X8_LE:
2856                 /* RGB565 {g[2:0],b[4:0]},{r[4:0],g[5:3]} */
2857                 fmt = 0x6F;
2858                 mux = OV5640_FMT_MUX_RGB;
2859                 break;
2860         case MEDIA_BUS_FMT_RGB565_2X8_BE:
2861                 /* RGB565 {r[4:0],g[5:3]},{g[2:0],b[4:0]} */
2862                 fmt = 0x61;
2863                 mux = OV5640_FMT_MUX_RGB;
2864                 break;
2865         case MEDIA_BUS_FMT_JPEG_1X8:
2866                 /* YUV422, YUYV */
2867                 fmt = 0x30;
2868                 mux = OV5640_FMT_MUX_YUV422;
2869                 is_jpeg = true;
2870                 break;
2871         case MEDIA_BUS_FMT_SBGGR8_1X8:
2872                 /* Raw, BGBG... / GRGR... */
2873                 fmt = 0x00;
2874                 mux = OV5640_FMT_MUX_RAW_DPC;
2875                 break;
2876         case MEDIA_BUS_FMT_SGBRG8_1X8:
2877                 /* Raw bayer, GBGB... / RGRG... */
2878                 fmt = 0x01;
2879                 mux = OV5640_FMT_MUX_RAW_DPC;
2880                 break;
2881         case MEDIA_BUS_FMT_SGRBG8_1X8:
2882                 /* Raw bayer, GRGR... / BGBG... */
2883                 fmt = 0x02;
2884                 mux = OV5640_FMT_MUX_RAW_DPC;
2885                 break;
2886         case MEDIA_BUS_FMT_SRGGB8_1X8:
2887                 /* Raw bayer, RGRG... / GBGB... */
2888                 fmt = 0x03;
2889                 mux = OV5640_FMT_MUX_RAW_DPC;
2890                 break;
2891         default:
2892                 return -EINVAL;
2893         }
2894
2895         /* FORMAT CONTROL00: YUV and RGB formatting */
2896         ret = ov5640_write_reg(sensor, OV5640_REG_FORMAT_CONTROL00, fmt);
2897         if (ret)
2898                 return ret;
2899
2900         /* FORMAT MUX CONTROL: ISP YUV or RGB */
2901         ret = ov5640_write_reg(sensor, OV5640_REG_ISP_FORMAT_MUX_CTRL, mux);
2902         if (ret)
2903                 return ret;
2904
2905         /*
2906          * TIMING TC REG21:
2907          * - [5]:       JPEG enable
2908          */
2909         ret = ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG21,
2910                              BIT(5), is_jpeg ? BIT(5) : 0);
2911         if (ret)
2912                 return ret;
2913
2914         /*
2915          * SYSTEM RESET02:
2916          * - [4]:       Reset JFIFO
2917          * - [3]:       Reset SFIFO
2918          * - [2]:       Reset JPEG
2919          */
2920         ret = ov5640_mod_reg(sensor, OV5640_REG_SYS_RESET02,
2921                              BIT(4) | BIT(3) | BIT(2),
2922                              is_jpeg ? 0 : (BIT(4) | BIT(3) | BIT(2)));
2923         if (ret)
2924                 return ret;
2925
2926         /*
2927          * CLOCK ENABLE02:
2928          * - [5]:       Enable JPEG 2x clock
2929          * - [3]:       Enable JPEG clock
2930          */
2931         return ov5640_mod_reg(sensor, OV5640_REG_SYS_CLOCK_ENABLE02,
2932                               BIT(5) | BIT(3),
2933                               is_jpeg ? (BIT(5) | BIT(3)) : 0);
2934 }
2935
2936 /*
2937  * Sensor Controls.
2938  */
2939
2940 static int ov5640_set_ctrl_hue(struct ov5640_dev *sensor, int value)
2941 {
2942         int ret;
2943
2944         if (value) {
2945                 ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0,
2946                                      BIT(0), BIT(0));
2947                 if (ret)
2948                         return ret;
2949                 ret = ov5640_write_reg16(sensor, OV5640_REG_SDE_CTRL1, value);
2950         } else {
2951                 ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0, BIT(0), 0);
2952         }
2953
2954         return ret;
2955 }
2956
2957 static int ov5640_set_ctrl_contrast(struct ov5640_dev *sensor, int value)
2958 {
2959         int ret;
2960
2961         if (value) {
2962                 ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0,
2963                                      BIT(2), BIT(2));
2964                 if (ret)
2965                         return ret;
2966                 ret = ov5640_write_reg(sensor, OV5640_REG_SDE_CTRL5,
2967                                        value & 0xff);
2968         } else {
2969                 ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0, BIT(2), 0);
2970         }
2971
2972         return ret;
2973 }
2974
2975 static int ov5640_set_ctrl_saturation(struct ov5640_dev *sensor, int value)
2976 {
2977         int ret;
2978
2979         if (value) {
2980                 ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0,
2981                                      BIT(1), BIT(1));
2982                 if (ret)
2983                         return ret;
2984                 ret = ov5640_write_reg(sensor, OV5640_REG_SDE_CTRL3,
2985                                        value & 0xff);
2986                 if (ret)
2987                         return ret;
2988                 ret = ov5640_write_reg(sensor, OV5640_REG_SDE_CTRL4,
2989                                        value & 0xff);
2990         } else {
2991                 ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0, BIT(1), 0);
2992         }
2993
2994         return ret;
2995 }
2996
2997 static int ov5640_set_ctrl_white_balance(struct ov5640_dev *sensor, int awb)
2998 {
2999         int ret;
3000
3001         ret = ov5640_mod_reg(sensor, OV5640_REG_AWB_MANUAL_CTRL,
3002                              BIT(0), awb ? 0 : 1);
3003         if (ret)
3004                 return ret;
3005
3006         if (!awb) {
3007                 u16 red = (u16)sensor->ctrls.red_balance->val;
3008                 u16 blue = (u16)sensor->ctrls.blue_balance->val;
3009
3010                 ret = ov5640_write_reg16(sensor, OV5640_REG_AWB_R_GAIN, red);
3011                 if (ret)
3012                         return ret;
3013                 ret = ov5640_write_reg16(sensor, OV5640_REG_AWB_B_GAIN, blue);
3014         }
3015
3016         return ret;
3017 }
3018
3019 static int ov5640_set_ctrl_exposure(struct ov5640_dev *sensor,
3020                                     enum v4l2_exposure_auto_type auto_exposure)
3021 {
3022         struct ov5640_ctrls *ctrls = &sensor->ctrls;
3023         bool auto_exp = (auto_exposure == V4L2_EXPOSURE_AUTO);
3024         int ret = 0;
3025
3026         if (ctrls->auto_exp->is_new) {
3027                 ret = ov5640_set_autoexposure(sensor, auto_exp);
3028                 if (ret)
3029                         return ret;
3030         }
3031
3032         if (!auto_exp && ctrls->exposure->is_new) {
3033                 u16 max_exp;
3034
3035                 ret = ov5640_read_reg16(sensor, OV5640_REG_AEC_PK_VTS,
3036                                         &max_exp);
3037                 if (ret)
3038                         return ret;
3039                 ret = ov5640_get_vts(sensor);
3040                 if (ret < 0)
3041                         return ret;
3042                 max_exp += ret;
3043                 ret = 0;
3044
3045                 if (ctrls->exposure->val < max_exp)
3046                         ret = ov5640_set_exposure(sensor, ctrls->exposure->val);
3047         }
3048
3049         return ret;
3050 }
3051
3052 static int ov5640_set_ctrl_gain(struct ov5640_dev *sensor, bool auto_gain)
3053 {
3054         struct ov5640_ctrls *ctrls = &sensor->ctrls;
3055         int ret = 0;
3056
3057         if (ctrls->auto_gain->is_new) {
3058                 ret = ov5640_set_autogain(sensor, auto_gain);
3059                 if (ret)
3060                         return ret;
3061         }
3062
3063         if (!auto_gain && ctrls->gain->is_new)
3064                 ret = ov5640_set_gain(sensor, ctrls->gain->val);
3065
3066         return ret;
3067 }
3068
3069 static const char * const test_pattern_menu[] = {
3070         "Disabled",
3071         "Color bars",
3072         "Color bars w/ rolling bar",
3073         "Color squares",
3074         "Color squares w/ rolling bar",
3075 };
3076
3077 #define OV5640_TEST_ENABLE              BIT(7)
3078 #define OV5640_TEST_ROLLING             BIT(6)  /* rolling horizontal bar */
3079 #define OV5640_TEST_TRANSPARENT         BIT(5)
3080 #define OV5640_TEST_SQUARE_BW           BIT(4)  /* black & white squares */
3081 #define OV5640_TEST_BAR_STANDARD        (0 << 2)
3082 #define OV5640_TEST_BAR_VERT_CHANGE_1   (1 << 2)
3083 #define OV5640_TEST_BAR_HOR_CHANGE      (2 << 2)
3084 #define OV5640_TEST_BAR_VERT_CHANGE_2   (3 << 2)
3085 #define OV5640_TEST_BAR                 (0 << 0)
3086 #define OV5640_TEST_RANDOM              (1 << 0)
3087 #define OV5640_TEST_SQUARE              (2 << 0)
3088 #define OV5640_TEST_BLACK               (3 << 0)
3089
3090 static const u8 test_pattern_val[] = {
3091         0,
3092         OV5640_TEST_ENABLE | OV5640_TEST_BAR_VERT_CHANGE_1 |
3093                 OV5640_TEST_BAR,
3094         OV5640_TEST_ENABLE | OV5640_TEST_ROLLING |
3095                 OV5640_TEST_BAR_VERT_CHANGE_1 | OV5640_TEST_BAR,
3096         OV5640_TEST_ENABLE | OV5640_TEST_SQUARE,
3097         OV5640_TEST_ENABLE | OV5640_TEST_ROLLING | OV5640_TEST_SQUARE,
3098 };
3099
3100 static int ov5640_set_ctrl_test_pattern(struct ov5640_dev *sensor, int value)
3101 {
3102         return ov5640_write_reg(sensor, OV5640_REG_PRE_ISP_TEST_SET1,
3103                                 test_pattern_val[value]);
3104 }
3105
3106 static int ov5640_set_ctrl_light_freq(struct ov5640_dev *sensor, int value)
3107 {
3108         int ret;
3109
3110         ret = ov5640_mod_reg(sensor, OV5640_REG_HZ5060_CTRL01, BIT(7),
3111                              (value == V4L2_CID_POWER_LINE_FREQUENCY_AUTO) ?
3112                              0 : BIT(7));
3113         if (ret)
3114                 return ret;
3115
3116         return ov5640_mod_reg(sensor, OV5640_REG_HZ5060_CTRL00, BIT(2),
3117                               (value == V4L2_CID_POWER_LINE_FREQUENCY_50HZ) ?
3118                               BIT(2) : 0);
3119 }
3120
3121 static int ov5640_set_ctrl_hflip(struct ov5640_dev *sensor, int value)
3122 {
3123         /*
3124          * If sensor is mounted upside down, mirror logic is inversed.
3125          *
3126          * Sensor is a BSI (Back Side Illuminated) one,
3127          * so image captured is physically mirrored.
3128          * This is why mirror logic is inversed in
3129          * order to cancel this mirror effect.
3130          */
3131
3132         /*
3133          * TIMING TC REG21:
3134          * - [2]:       ISP mirror
3135          * - [1]:       Sensor mirror
3136          */
3137         return ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG21,
3138                               BIT(2) | BIT(1),
3139                               (!(value ^ sensor->upside_down)) ?
3140                               (BIT(2) | BIT(1)) : 0);
3141 }
3142
3143 static int ov5640_set_ctrl_vflip(struct ov5640_dev *sensor, int value)
3144 {
3145         /* If sensor is mounted upside down, flip logic is inversed */
3146
3147         /*
3148          * TIMING TC REG20:
3149          * - [2]:       ISP vflip
3150          * - [1]:       Sensor vflip
3151          */
3152         return ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG20,
3153                               BIT(2) | BIT(1),
3154                               (value ^ sensor->upside_down) ?
3155                               (BIT(2) | BIT(1)) : 0);
3156 }
3157
3158 static int ov5640_set_ctrl_vblank(struct ov5640_dev *sensor, int value)
3159 {
3160         const struct ov5640_mode_info *mode = sensor->current_mode;
3161
3162         /* Update the VTOT timing register value. */
3163         return ov5640_write_reg16(sensor, OV5640_REG_TIMING_VTS,
3164                                   mode->height + value);
3165 }
3166
3167 static int ov5640_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
3168 {
3169         struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
3170         struct ov5640_dev *sensor = to_ov5640_dev(sd);
3171         int val;
3172
3173         /* v4l2_ctrl_lock() locks our own mutex */
3174
3175         switch (ctrl->id) {
3176         case V4L2_CID_AUTOGAIN:
3177                 val = ov5640_get_gain(sensor);
3178                 if (val < 0)
3179                         return val;
3180                 sensor->ctrls.gain->val = val;
3181                 break;
3182         case V4L2_CID_EXPOSURE_AUTO:
3183                 val = ov5640_get_exposure(sensor);
3184                 if (val < 0)
3185                         return val;
3186                 sensor->ctrls.exposure->val = val;
3187                 break;
3188         }
3189
3190         return 0;
3191 }
3192
3193 static int ov5640_s_ctrl(struct v4l2_ctrl *ctrl)
3194 {
3195         struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
3196         struct ov5640_dev *sensor = to_ov5640_dev(sd);
3197         const struct ov5640_mode_info *mode = sensor->current_mode;
3198         const struct ov5640_timings *timings;
3199         unsigned int exp_max;
3200         int ret;
3201
3202         /* v4l2_ctrl_lock() locks our own mutex */
3203
3204         switch (ctrl->id) {
3205         case V4L2_CID_VBLANK:
3206                 /* Update the exposure range to the newly programmed vblank. */
3207                 timings = ov5640_timings(sensor, mode);
3208                 exp_max = mode->height + ctrl->val - 4;
3209                 __v4l2_ctrl_modify_range(sensor->ctrls.exposure,
3210                                          sensor->ctrls.exposure->minimum,
3211                                          exp_max, sensor->ctrls.exposure->step,
3212                                          timings->vblank_def);
3213                 break;
3214         }
3215
3216         /*
3217          * If the device is not powered up by the host driver do
3218          * not apply any controls to H/W at this time. Instead
3219          * the controls will be restored right after power-up.
3220          */
3221         if (sensor->power_count == 0)
3222                 return 0;
3223
3224         switch (ctrl->id) {
3225         case V4L2_CID_AUTOGAIN:
3226                 ret = ov5640_set_ctrl_gain(sensor, ctrl->val);
3227                 break;
3228         case V4L2_CID_EXPOSURE_AUTO:
3229                 ret = ov5640_set_ctrl_exposure(sensor, ctrl->val);
3230                 break;
3231         case V4L2_CID_AUTO_WHITE_BALANCE:
3232                 ret = ov5640_set_ctrl_white_balance(sensor, ctrl->val);
3233                 break;
3234         case V4L2_CID_HUE:
3235                 ret = ov5640_set_ctrl_hue(sensor, ctrl->val);
3236                 break;
3237         case V4L2_CID_CONTRAST:
3238                 ret = ov5640_set_ctrl_contrast(sensor, ctrl->val);
3239                 break;
3240         case V4L2_CID_SATURATION:
3241                 ret = ov5640_set_ctrl_saturation(sensor, ctrl->val);
3242                 break;
3243         case V4L2_CID_TEST_PATTERN:
3244                 ret = ov5640_set_ctrl_test_pattern(sensor, ctrl->val);
3245                 break;
3246         case V4L2_CID_POWER_LINE_FREQUENCY:
3247                 ret = ov5640_set_ctrl_light_freq(sensor, ctrl->val);
3248                 break;
3249         case V4L2_CID_HFLIP:
3250                 ret = ov5640_set_ctrl_hflip(sensor, ctrl->val);
3251                 break;
3252         case V4L2_CID_VFLIP:
3253                 ret = ov5640_set_ctrl_vflip(sensor, ctrl->val);
3254                 break;
3255         case V4L2_CID_VBLANK:
3256                 ret = ov5640_set_ctrl_vblank(sensor, ctrl->val);
3257                 break;
3258         default:
3259                 ret = -EINVAL;
3260                 break;
3261         }
3262
3263         return ret;
3264 }
3265
3266 static const struct v4l2_ctrl_ops ov5640_ctrl_ops = {
3267         .g_volatile_ctrl = ov5640_g_volatile_ctrl,
3268         .s_ctrl = ov5640_s_ctrl,
3269 };
3270
3271 static int ov5640_init_controls(struct ov5640_dev *sensor)
3272 {
3273         const struct ov5640_mode_info *mode = sensor->current_mode;
3274         const struct v4l2_ctrl_ops *ops = &ov5640_ctrl_ops;
3275         struct ov5640_ctrls *ctrls = &sensor->ctrls;
3276         struct v4l2_ctrl_handler *hdl = &ctrls->handler;
3277         const struct ov5640_timings *timings;
3278         unsigned int max_vblank;
3279         unsigned int hblank;
3280         int ret;
3281
3282         v4l2_ctrl_handler_init(hdl, 32);
3283
3284         /* we can use our own mutex for the ctrl lock */
3285         hdl->lock = &sensor->lock;
3286
3287         /* Clock related controls */
3288         ctrls->pixel_rate = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
3289                               ov5640_pixel_rates[OV5640_NUM_PIXEL_RATES - 1],
3290                               ov5640_pixel_rates[0], 1,
3291                               ov5640_pixel_rates[mode->pixel_rate]);
3292
3293         ctrls->link_freq = v4l2_ctrl_new_int_menu(hdl, ops,
3294                                         V4L2_CID_LINK_FREQ,
3295                                         ARRAY_SIZE(ov5640_csi2_link_freqs) - 1,
3296                                         OV5640_DEFAULT_LINK_FREQ,
3297                                         ov5640_csi2_link_freqs);
3298
3299         timings = ov5640_timings(sensor, mode);
3300         hblank = timings->htot - mode->width;
3301         ctrls->hblank = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HBLANK, hblank,
3302                                           hblank, 1, hblank);
3303
3304         max_vblank = OV5640_MAX_VTS - mode->height;
3305         ctrls->vblank = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VBLANK,
3306                                           OV5640_MIN_VBLANK, max_vblank,
3307                                           1, timings->vblank_def);
3308
3309         /* Auto/manual white balance */
3310         ctrls->auto_wb = v4l2_ctrl_new_std(hdl, ops,
3311                                            V4L2_CID_AUTO_WHITE_BALANCE,
3312                                            0, 1, 1, 1);
3313         ctrls->blue_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BLUE_BALANCE,
3314                                                 0, 4095, 1, 0);
3315         ctrls->red_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_RED_BALANCE,
3316                                                0, 4095, 1, 0);
3317         /* Auto/manual exposure */
3318         ctrls->auto_exp = v4l2_ctrl_new_std_menu(hdl, ops,
3319                                                  V4L2_CID_EXPOSURE_AUTO,
3320                                                  V4L2_EXPOSURE_MANUAL, 0,
3321                                                  V4L2_EXPOSURE_AUTO);
3322         ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
3323                                             0, 65535, 1, 0);
3324         /* Auto/manual gain */
3325         ctrls->auto_gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTOGAIN,
3326                                              0, 1, 1, 1);
3327         ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN,
3328                                         0, 1023, 1, 0);
3329
3330         ctrls->saturation = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SATURATION,
3331                                               0, 255, 1, 64);
3332         ctrls->hue = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HUE,
3333                                        0, 359, 1, 0);
3334         ctrls->contrast = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_CONTRAST,
3335                                             0, 255, 1, 0);
3336         ctrls->test_pattern =
3337                 v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
3338                                              ARRAY_SIZE(test_pattern_menu) - 1,
3339                                              0, 0, test_pattern_menu);
3340         ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP,
3341                                          0, 1, 1, 0);
3342         ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP,
3343                                          0, 1, 1, 0);
3344
3345         ctrls->light_freq =
3346                 v4l2_ctrl_new_std_menu(hdl, ops,
3347                                        V4L2_CID_POWER_LINE_FREQUENCY,
3348                                        V4L2_CID_POWER_LINE_FREQUENCY_AUTO, 0,
3349                                        V4L2_CID_POWER_LINE_FREQUENCY_50HZ);
3350
3351         if (hdl->error) {
3352                 ret = hdl->error;
3353                 goto free_ctrls;
3354         }
3355
3356         ctrls->pixel_rate->flags |= V4L2_CTRL_FLAG_READ_ONLY;
3357         ctrls->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
3358         ctrls->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
3359         ctrls->gain->flags |= V4L2_CTRL_FLAG_VOLATILE;
3360         ctrls->exposure->flags |= V4L2_CTRL_FLAG_VOLATILE;
3361
3362         v4l2_ctrl_auto_cluster(3, &ctrls->auto_wb, 0, false);
3363         v4l2_ctrl_auto_cluster(2, &ctrls->auto_gain, 0, true);
3364         v4l2_ctrl_auto_cluster(2, &ctrls->auto_exp, 1, true);
3365
3366         sensor->sd.ctrl_handler = hdl;
3367         return 0;
3368
3369 free_ctrls:
3370         v4l2_ctrl_handler_free(hdl);
3371         return ret;
3372 }
3373
3374 static int ov5640_enum_frame_size(struct v4l2_subdev *sd,
3375                                   struct v4l2_subdev_state *sd_state,
3376                                   struct v4l2_subdev_frame_size_enum *fse)
3377 {
3378         if (fse->pad != 0)
3379                 return -EINVAL;
3380         if (fse->index >= OV5640_NUM_MODES)
3381                 return -EINVAL;
3382
3383         fse->min_width = ov5640_mode_data[fse->index].width;
3384         fse->max_width = fse->min_width;
3385         fse->min_height = ov5640_mode_data[fse->index].height;
3386         fse->max_height = fse->min_height;
3387
3388         return 0;
3389 }
3390
3391 static int ov5640_enum_frame_interval(
3392         struct v4l2_subdev *sd,
3393         struct v4l2_subdev_state *sd_state,
3394         struct v4l2_subdev_frame_interval_enum *fie)
3395 {
3396         struct ov5640_dev *sensor = to_ov5640_dev(sd);
3397         struct v4l2_fract tpf;
3398         int ret;
3399
3400         if (fie->pad != 0)
3401                 return -EINVAL;
3402         if (fie->index >= OV5640_NUM_FRAMERATES)
3403                 return -EINVAL;
3404
3405         tpf.numerator = 1;
3406         tpf.denominator = ov5640_framerates[fie->index];
3407
3408         ret = ov5640_try_frame_interval(sensor, &tpf,
3409                                         fie->width, fie->height);
3410         if (ret < 0)
3411                 return -EINVAL;
3412
3413         fie->interval = tpf;
3414         return 0;
3415 }
3416
3417 static int ov5640_g_frame_interval(struct v4l2_subdev *sd,
3418                                    struct v4l2_subdev_frame_interval *fi)
3419 {
3420         struct ov5640_dev *sensor = to_ov5640_dev(sd);
3421
3422         mutex_lock(&sensor->lock);
3423         fi->interval = sensor->frame_interval;
3424         mutex_unlock(&sensor->lock);
3425
3426         return 0;
3427 }
3428
3429 static int ov5640_s_frame_interval(struct v4l2_subdev *sd,
3430                                    struct v4l2_subdev_frame_interval *fi)
3431 {
3432         struct ov5640_dev *sensor = to_ov5640_dev(sd);
3433         const struct ov5640_mode_info *mode;
3434         int frame_rate, ret = 0;
3435
3436         if (fi->pad != 0)
3437                 return -EINVAL;
3438
3439         mutex_lock(&sensor->lock);
3440
3441         if (sensor->streaming) {
3442                 ret = -EBUSY;
3443                 goto out;
3444         }
3445
3446         mode = sensor->current_mode;
3447
3448         frame_rate = ov5640_try_frame_interval(sensor, &fi->interval,
3449                                                mode->width,
3450                                                mode->height);
3451         if (frame_rate < 0) {
3452                 /* Always return a valid frame interval value */
3453                 fi->interval = sensor->frame_interval;
3454                 goto out;
3455         }
3456
3457         mode = ov5640_find_mode(sensor, frame_rate, mode->width,
3458                                 mode->height, true);
3459         if (!mode) {
3460                 ret = -EINVAL;
3461                 goto out;
3462         }
3463
3464         if (mode != sensor->current_mode ||
3465             frame_rate != sensor->current_fr) {
3466                 sensor->current_fr = frame_rate;
3467                 sensor->frame_interval = fi->interval;
3468                 sensor->current_mode = mode;
3469                 sensor->pending_mode_change = true;
3470
3471                 __v4l2_ctrl_s_ctrl_int64(sensor->ctrls.pixel_rate,
3472                                          ov5640_calc_pixel_rate(sensor));
3473         }
3474 out:
3475         mutex_unlock(&sensor->lock);
3476         return ret;
3477 }
3478
3479 static int ov5640_enum_mbus_code(struct v4l2_subdev *sd,
3480                                  struct v4l2_subdev_state *sd_state,
3481                                  struct v4l2_subdev_mbus_code_enum *code)
3482 {
3483         if (code->pad != 0)
3484                 return -EINVAL;
3485         if (code->index >= ARRAY_SIZE(ov5640_formats))
3486                 return -EINVAL;
3487
3488         code->code = ov5640_formats[code->index].code;
3489         return 0;
3490 }
3491
3492 static int ov5640_s_stream(struct v4l2_subdev *sd, int enable)
3493 {
3494         struct ov5640_dev *sensor = to_ov5640_dev(sd);
3495         int ret = 0;
3496
3497         mutex_lock(&sensor->lock);
3498
3499         if (sensor->streaming == !enable) {
3500                 if (enable && sensor->pending_mode_change) {
3501                         ret = ov5640_set_mode(sensor);
3502                         if (ret)
3503                                 goto out;
3504                 }
3505
3506                 if (enable && sensor->pending_fmt_change) {
3507                         ret = ov5640_set_framefmt(sensor, &sensor->fmt);
3508                         if (ret)
3509                                 goto out;
3510                         sensor->pending_fmt_change = false;
3511                 }
3512
3513                 if (ov5640_is_csi2(sensor))
3514                         ret = ov5640_set_stream_mipi(sensor, enable);
3515                 else
3516                         ret = ov5640_set_stream_dvp(sensor, enable);
3517
3518                 if (!ret)
3519                         sensor->streaming = enable;
3520         }
3521 out:
3522         mutex_unlock(&sensor->lock);
3523         return ret;
3524 }
3525
3526 static const struct v4l2_subdev_core_ops ov5640_core_ops = {
3527         .s_power = ov5640_s_power,
3528         .log_status = v4l2_ctrl_subdev_log_status,
3529         .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
3530         .unsubscribe_event = v4l2_event_subdev_unsubscribe,
3531 };
3532
3533 static const struct v4l2_subdev_video_ops ov5640_video_ops = {
3534         .g_frame_interval = ov5640_g_frame_interval,
3535         .s_frame_interval = ov5640_s_frame_interval,
3536         .s_stream = ov5640_s_stream,
3537 };
3538
3539 static const struct v4l2_subdev_pad_ops ov5640_pad_ops = {
3540         .enum_mbus_code = ov5640_enum_mbus_code,
3541         .get_fmt = ov5640_get_fmt,
3542         .set_fmt = ov5640_set_fmt,
3543         .enum_frame_size = ov5640_enum_frame_size,
3544         .enum_frame_interval = ov5640_enum_frame_interval,
3545 };
3546
3547 static const struct v4l2_subdev_ops ov5640_subdev_ops = {
3548         .core = &ov5640_core_ops,
3549         .video = &ov5640_video_ops,
3550         .pad = &ov5640_pad_ops,
3551 };
3552
3553 static int ov5640_get_regulators(struct ov5640_dev *sensor)
3554 {
3555         int i;
3556
3557         for (i = 0; i < OV5640_NUM_SUPPLIES; i++)
3558                 sensor->supplies[i].supply = ov5640_supply_name[i];
3559
3560         return devm_regulator_bulk_get(&sensor->i2c_client->dev,
3561                                        OV5640_NUM_SUPPLIES,
3562                                        sensor->supplies);
3563 }
3564
3565 static int ov5640_check_chip_id(struct ov5640_dev *sensor)
3566 {
3567         struct i2c_client *client = sensor->i2c_client;
3568         int ret = 0;
3569         u16 chip_id;
3570
3571         ret = ov5640_set_power_on(sensor);
3572         if (ret)
3573                 return ret;
3574
3575         ret = ov5640_read_reg16(sensor, OV5640_REG_CHIP_ID, &chip_id);
3576         if (ret) {
3577                 dev_err(&client->dev, "%s: failed to read chip identifier\n",
3578                         __func__);
3579                 goto power_off;
3580         }
3581
3582         if (chip_id != 0x5640) {
3583                 dev_err(&client->dev, "%s: wrong chip identifier, expected 0x5640, got 0x%x\n",
3584                         __func__, chip_id);
3585                 ret = -ENXIO;
3586         }
3587
3588 power_off:
3589         ov5640_set_power_off(sensor);
3590         return ret;
3591 }
3592
3593 static int ov5640_probe(struct i2c_client *client)
3594 {
3595         struct device *dev = &client->dev;
3596         struct fwnode_handle *endpoint;
3597         struct ov5640_dev *sensor;
3598         struct v4l2_mbus_framefmt *fmt;
3599         u32 rotation;
3600         int ret;
3601
3602         sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
3603         if (!sensor)
3604                 return -ENOMEM;
3605
3606         sensor->i2c_client = client;
3607
3608         /*
3609          * default init sequence initialize sensor to
3610          * YUV422 UYVY VGA@30fps
3611          */
3612         fmt = &sensor->fmt;
3613         fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
3614         fmt->colorspace = V4L2_COLORSPACE_SRGB;
3615         fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
3616         fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
3617         fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
3618         fmt->width = 640;
3619         fmt->height = 480;
3620         fmt->field = V4L2_FIELD_NONE;
3621         sensor->frame_interval.numerator = 1;
3622         sensor->frame_interval.denominator = ov5640_framerates[OV5640_30_FPS];
3623         sensor->current_fr = OV5640_30_FPS;
3624         sensor->current_mode =
3625                 &ov5640_mode_data[OV5640_MODE_VGA_640_480];
3626         sensor->last_mode = sensor->current_mode;
3627         sensor->current_link_freq = OV5640_DEFAULT_LINK_FREQ;
3628
3629         sensor->ae_target = 52;
3630
3631         /* optional indication of physical rotation of sensor */
3632         ret = fwnode_property_read_u32(dev_fwnode(&client->dev), "rotation",
3633                                        &rotation);
3634         if (!ret) {
3635                 switch (rotation) {
3636                 case 180:
3637                         sensor->upside_down = true;
3638                         fallthrough;
3639                 case 0:
3640                         break;
3641                 default:
3642                         dev_warn(dev, "%u degrees rotation is not supported, ignoring...\n",
3643                                  rotation);
3644                 }
3645         }
3646
3647         endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(&client->dev),
3648                                                   NULL);
3649         if (!endpoint) {
3650                 dev_err(dev, "endpoint node not found\n");
3651                 return -EINVAL;
3652         }
3653
3654         ret = v4l2_fwnode_endpoint_parse(endpoint, &sensor->ep);
3655         fwnode_handle_put(endpoint);
3656         if (ret) {
3657                 dev_err(dev, "Could not parse endpoint\n");
3658                 return ret;
3659         }
3660
3661         if (sensor->ep.bus_type != V4L2_MBUS_PARALLEL &&
3662             sensor->ep.bus_type != V4L2_MBUS_CSI2_DPHY &&
3663             sensor->ep.bus_type != V4L2_MBUS_BT656) {
3664                 dev_err(dev, "Unsupported bus type %d\n", sensor->ep.bus_type);
3665                 return -EINVAL;
3666         }
3667
3668         /* get system clock (xclk) */
3669         sensor->xclk = devm_clk_get(dev, "xclk");
3670         if (IS_ERR(sensor->xclk)) {
3671                 dev_err(dev, "failed to get xclk\n");
3672                 return PTR_ERR(sensor->xclk);
3673         }
3674
3675         sensor->xclk_freq = clk_get_rate(sensor->xclk);
3676         if (sensor->xclk_freq < OV5640_XCLK_MIN ||
3677             sensor->xclk_freq > OV5640_XCLK_MAX) {
3678                 dev_err(dev, "xclk frequency out of range: %d Hz\n",
3679                         sensor->xclk_freq);
3680                 return -EINVAL;
3681         }
3682
3683         /* request optional power down pin */
3684         sensor->pwdn_gpio = devm_gpiod_get_optional(dev, "powerdown",
3685                                                     GPIOD_OUT_HIGH);
3686         if (IS_ERR(sensor->pwdn_gpio))
3687                 return PTR_ERR(sensor->pwdn_gpio);
3688
3689         /* request optional reset pin */
3690         sensor->reset_gpio = devm_gpiod_get_optional(dev, "reset",
3691                                                      GPIOD_OUT_HIGH);
3692         if (IS_ERR(sensor->reset_gpio))
3693                 return PTR_ERR(sensor->reset_gpio);
3694
3695         v4l2_i2c_subdev_init(&sensor->sd, client, &ov5640_subdev_ops);
3696
3697         sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
3698                             V4L2_SUBDEV_FL_HAS_EVENTS;
3699         sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
3700         sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
3701         ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
3702         if (ret)
3703                 return ret;
3704
3705         ret = ov5640_get_regulators(sensor);
3706         if (ret)
3707                 return ret;
3708
3709         mutex_init(&sensor->lock);
3710
3711         ret = ov5640_check_chip_id(sensor);
3712         if (ret)
3713                 goto entity_cleanup;
3714
3715         ret = ov5640_init_controls(sensor);
3716         if (ret)
3717                 goto entity_cleanup;
3718
3719         ret = v4l2_async_register_subdev_sensor(&sensor->sd);
3720         if (ret)
3721                 goto free_ctrls;
3722
3723         return 0;
3724
3725 free_ctrls:
3726         v4l2_ctrl_handler_free(&sensor->ctrls.handler);
3727 entity_cleanup:
3728         media_entity_cleanup(&sensor->sd.entity);
3729         mutex_destroy(&sensor->lock);
3730         return ret;
3731 }
3732
3733 static int ov5640_remove(struct i2c_client *client)
3734 {
3735         struct v4l2_subdev *sd = i2c_get_clientdata(client);
3736         struct ov5640_dev *sensor = to_ov5640_dev(sd);
3737
3738         v4l2_async_unregister_subdev(&sensor->sd);
3739         media_entity_cleanup(&sensor->sd.entity);
3740         v4l2_ctrl_handler_free(&sensor->ctrls.handler);
3741         mutex_destroy(&sensor->lock);
3742
3743         return 0;
3744 }
3745
3746 static const struct i2c_device_id ov5640_id[] = {
3747         {"ov5640", 0},
3748         {},
3749 };
3750 MODULE_DEVICE_TABLE(i2c, ov5640_id);
3751
3752 static const struct of_device_id ov5640_dt_ids[] = {
3753         { .compatible = "ovti,ov5640" },
3754         { /* sentinel */ }
3755 };
3756 MODULE_DEVICE_TABLE(of, ov5640_dt_ids);
3757
3758 static struct i2c_driver ov5640_i2c_driver = {
3759         .driver = {
3760                 .name  = "ov5640",
3761                 .of_match_table = ov5640_dt_ids,
3762         },
3763         .id_table = ov5640_id,
3764         .probe_new = ov5640_probe,
3765         .remove   = ov5640_remove,
3766 };
3767
3768 module_i2c_driver(ov5640_i2c_driver);
3769
3770 MODULE_DESCRIPTION("OV5640 MIPI Camera Subdev Driver");
3771 MODULE_LICENSE("GPL");