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