drivers/media/i2c/ov5640.c

   1 /*
   2  * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved.
   3  * Copyright (C) 2014-2017 Mentor Graphics Inc.
   4  *
   5  * This program is free software; you can redistribute it and/or modify
   6  * it under the terms of the GNU General Public License as published by
   7  * the Free Software Foundation; either version 2 of the License, or
   8  * (at your option) any later version.
   9  */
  10
  11 #include <linux/clk.h>
  12 #include <linux/clk-provider.h>
  13 #include <linux/clkdev.h>
  14 #include <linux/ctype.h>
  15 #include <linux/delay.h>
  16 #include <linux/device.h>
  17 #include <linux/gpio/consumer.h>
  18 #include <linux/i2c.h>
  19 #include <linux/init.h>
  20 #include <linux/module.h>
  21 #include <linux/of_device.h>
  22 #include <linux/regulator/consumer.h>
  23 #include <linux/slab.h>
  24 #include <linux/types.h>
  25 #include <media/v4l2-async.h>
  26 #include <media/v4l2-ctrls.h>
  27 #include <media/v4l2-device.h>
  28 #include <media/v4l2-fwnode.h>
  29 #include <media/v4l2-subdev.h>
  30
  31 /* min/typical/max system clock (xclk) frequencies */
  32 #define OV5640_XCLK_MIN  6000000
  33 #define OV5640_XCLK_MAX 24000000
  34
  35 #define OV5640_DEFAULT_SLAVE_ID 0x3c
  36
  37 #define OV5640_REG_SYS_RESET02          0x3002
  38 #define OV5640_REG_SYS_CLOCK_ENABLE02   0x3006
  39 #define OV5640_REG_SYS_CTRL0            0x3008
  40 #define OV5640_REG_CHIP_ID              0x300a
  41 #define OV5640_REG_IO_MIPI_CTRL00       0x300e
  42 #define OV5640_REG_PAD_OUTPUT_ENABLE01  0x3017
  43 #define OV5640_REG_PAD_OUTPUT_ENABLE02  0x3018
  44 #define OV5640_REG_PAD_OUTPUT00         0x3019
  45 #define OV5640_REG_SYSTEM_CONTROL1      0x302e
  46 #define OV5640_REG_SC_PLL_CTRL0         0x3034
  47 #define OV5640_REG_SC_PLL_CTRL1         0x3035
  48 #define OV5640_REG_SC_PLL_CTRL2         0x3036
  49 #define OV5640_REG_SC_PLL_CTRL3         0x3037
  50 #define OV5640_REG_SLAVE_ID             0x3100
  51 #define OV5640_REG_SCCB_SYS_CTRL1       0x3103
  52 #define OV5640_REG_SYS_ROOT_DIVIDER     0x3108
  53 #define OV5640_REG_AWB_R_GAIN           0x3400
  54 #define OV5640_REG_AWB_G_GAIN           0x3402
  55 #define OV5640_REG_AWB_B_GAIN           0x3404
  56 #define OV5640_REG_AWB_MANUAL_CTRL      0x3406
  57 #define OV5640_REG_AEC_PK_EXPOSURE_HI   0x3500
  58 #define OV5640_REG_AEC_PK_EXPOSURE_MED  0x3501
  59 #define OV5640_REG_AEC_PK_EXPOSURE_LO   0x3502
  60 #define OV5640_REG_AEC_PK_MANUAL        0x3503
  61 #define OV5640_REG_AEC_PK_REAL_GAIN     0x350a
  62 #define OV5640_REG_AEC_PK_VTS           0x350c
  63 #define OV5640_REG_TIMING_DVPHO         0x3808
  64 #define OV5640_REG_TIMING_DVPVO         0x380a
  65 #define OV5640_REG_TIMING_HTS           0x380c
  66 #define OV5640_REG_TIMING_VTS           0x380e
  67 #define OV5640_REG_TIMING_TC_REG20      0x3820
  68 #define OV5640_REG_TIMING_TC_REG21      0x3821
  69 #define OV5640_REG_AEC_CTRL00           0x3a00
  70 #define OV5640_REG_AEC_B50_STEP         0x3a08
  71 #define OV5640_REG_AEC_B60_STEP         0x3a0a
  72 #define OV5640_REG_AEC_CTRL0D           0x3a0d
  73 #define OV5640_REG_AEC_CTRL0E           0x3a0e
  74 #define OV5640_REG_AEC_CTRL0F           0x3a0f
  75 #define OV5640_REG_AEC_CTRL10           0x3a10
  76 #define OV5640_REG_AEC_CTRL11           0x3a11
  77 #define OV5640_REG_AEC_CTRL1B           0x3a1b
  78 #define OV5640_REG_AEC_CTRL1E           0x3a1e
  79 #define OV5640_REG_AEC_CTRL1F           0x3a1f
  80 #define OV5640_REG_HZ5060_CTRL00        0x3c00
  81 #define OV5640_REG_HZ5060_CTRL01        0x3c01
  82 #define OV5640_REG_SIGMADELTA_CTRL0C    0x3c0c
  83 #define OV5640_REG_FRAME_CTRL01         0x4202
  84 #define OV5640_REG_FORMAT_CONTROL00     0x4300
  85 #define OV5640_REG_POLARITY_CTRL00      0x4740
  86 #define OV5640_REG_MIPI_CTRL00          0x4800
  87 #define OV5640_REG_DEBUG_MODE           0x4814
  88 #define OV5640_REG_ISP_FORMAT_MUX_CTRL  0x501f
  89 #define OV5640_REG_PRE_ISP_TEST_SET1    0x503d
  90 #define OV5640_REG_SDE_CTRL0            0x5580
  91 #define OV5640_REG_SDE_CTRL1            0x5581
  92 #define OV5640_REG_SDE_CTRL3            0x5583
  93 #define OV5640_REG_SDE_CTRL4            0x5584
  94 #define OV5640_REG_SDE_CTRL5            0x5585
  95 #define OV5640_REG_AVG_READOUT          0x56a1
  96
  97 #define OV5640_SCLK2X_ROOT_DIVIDER_DEFAULT      1
  98 #define OV5640_SCLK_ROOT_DIVIDER_DEFAULT        2
  99
 100 enum ov5640_mode_id {
 101         OV5640_MODE_QCIF_176_144 = 0,
 102         OV5640_MODE_QVGA_320_240,
 103         OV5640_MODE_VGA_640_480,
 104         OV5640_MODE_NTSC_720_480,
 105         OV5640_MODE_PAL_720_576,
 106         OV5640_MODE_XGA_1024_768,
 107         OV5640_MODE_720P_1280_720,
 108         OV5640_MODE_1080P_1920_1080,
 109         OV5640_MODE_QSXGA_2592_1944,
 110         OV5640_NUM_MODES,
 111 };
 112
 113 enum ov5640_frame_rate {
 114         OV5640_15_FPS = 0,
 115         OV5640_30_FPS,
 116         OV5640_NUM_FRAMERATES,
 117 };
 118
 119 struct ov5640_pixfmt {
 120         u32 code;
 121         u32 colorspace;
 122 };
 123
 124 static const struct ov5640_pixfmt ov5640_formats[] = {
 125         { MEDIA_BUS_FMT_JPEG_1X8, V4L2_COLORSPACE_JPEG, },
 126         { MEDIA_BUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_SRGB, },
 127         { MEDIA_BUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_SRGB, },
 128         { MEDIA_BUS_FMT_RGB565_2X8_LE, V4L2_COLORSPACE_SRGB, },
 129         { MEDIA_BUS_FMT_RGB565_2X8_BE, V4L2_COLORSPACE_SRGB, },
 130 };
 131
 132 /*
 133  * FIXME: remove this when a subdev API becomes available
 134  * to set the MIPI CSI-2 virtual channel.
 135  */
 136 static unsigned int virtual_channel;
 137 module_param(virtual_channel, uint, 0444);
 138 MODULE_PARM_DESC(virtual_channel,
 139                  "MIPI CSI-2 virtual channel (0..3), default 0");
 140
 141 static const int ov5640_framerates[] = {
 142         [OV5640_15_FPS] = 15,
 143         [OV5640_30_FPS] = 30,
 144 };
 145
 146 /* regulator supplies */
 147 static const char * const ov5640_supply_name[] = {
 148         "DOVDD", /* Digital I/O (1.8V) supply */
 149         "DVDD",  /* Digital Core (1.5V) supply */
 150         "AVDD",  /* Analog (2.8V) supply */
 151 };
 152
 153 #define OV5640_NUM_SUPPLIES ARRAY_SIZE(ov5640_supply_name)
 154
 155 /*
 156  * Image size under 1280 * 960 are SUBSAMPLING
 157  * Image size upper 1280 * 960 are SCALING
 158  */
 159 enum ov5640_downsize_mode {
 160         SUBSAMPLING,
 161         SCALING,
 162 };
 163
 164 struct reg_value {
 165         u16 reg_addr;
 166         u8 val;
 167         u8 mask;
 168         u32 delay_ms;
 169 };
 170
 171 struct ov5640_mode_info {
 172         enum ov5640_mode_id id;
 173         enum ov5640_downsize_mode dn_mode;
 174         u32 hact;
 175         u32 htot;
 176         u32 vact;
 177         u32 vtot;
 178         const struct reg_value *reg_data;
 179         u32 reg_data_size;
 180 };
 181
 182 struct ov5640_ctrls {
 183         struct v4l2_ctrl_handler handler;
 184         struct {
 185                 struct v4l2_ctrl *auto_exp;
 186                 struct v4l2_ctrl *exposure;
 187         };
 188         struct {
 189                 struct v4l2_ctrl *auto_wb;
 190                 struct v4l2_ctrl *blue_balance;
 191                 struct v4l2_ctrl *red_balance;
 192         };
 193         struct {
 194                 struct v4l2_ctrl *auto_gain;
 195                 struct v4l2_ctrl *gain;
 196         };
 197         struct v4l2_ctrl *brightness;
 198         struct v4l2_ctrl *light_freq;
 199         struct v4l2_ctrl *saturation;
 200         struct v4l2_ctrl *contrast;
 201         struct v4l2_ctrl *hue;
 202         struct v4l2_ctrl *test_pattern;
 203         struct v4l2_ctrl *hflip;
 204         struct v4l2_ctrl *vflip;
 205 };
 206
 207 struct ov5640_dev {
 208         struct i2c_client *i2c_client;
 209         struct v4l2_subdev sd;
 210         struct media_pad pad;
 211         struct v4l2_fwnode_endpoint ep; /* the parsed DT endpoint info */
 212         struct clk *xclk; /* system clock to OV5640 */
 213         u32 xclk_freq;
 214
 215         struct regulator_bulk_data supplies[OV5640_NUM_SUPPLIES];
 216         struct gpio_desc *reset_gpio;
 217         struct gpio_desc *pwdn_gpio;
 218         bool   upside_down;
 219
 220         /* lock to protect all members below */
 221         struct mutex lock;
 222
 223         int power_count;
 224
 225         struct v4l2_mbus_framefmt fmt;
 226
 227         const struct ov5640_mode_info *current_mode;
 228         enum ov5640_frame_rate current_fr;
 229         struct v4l2_fract frame_interval;
 230
 231         struct ov5640_ctrls ctrls;
 232
 233         u32 prev_sysclk, prev_hts;
 234         u32 ae_low, ae_high, ae_target;
 235
 236         bool pending_mode_change;
 237         bool streaming;
 238 };
 239
 240 static inline struct ov5640_dev *to_ov5640_dev(struct v4l2_subdev *sd)
 241 {
 242         return container_of(sd, struct ov5640_dev, sd);
 243 }
 244
 245 static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
 246 {
 247         return &container_of(ctrl->handler, struct ov5640_dev,
 248                              ctrls.handler)->sd;
 249 }
 250
 251 /*
 252  * FIXME: all of these register tables are likely filled with
 253  * entries that set the register to their power-on default values,
 254  * and which are otherwise not touched by this driver. Those entries
 255  * should be identified and removed to speed register load time
 256  * over i2c.
 257  */
 258
 259 static const struct reg_value ov5640_init_setting_30fps_VGA[] = {
 260         {0x3103, 0x11, 0, 0}, {0x3008, 0x82, 0, 5}, {0x3008, 0x42, 0, 0},
 261         {0x3103, 0x03, 0, 0}, {0x3017, 0x00, 0, 0}, {0x3018, 0x00, 0, 0},
 262         {0x3034, 0x18, 0, 0}, {0x3035, 0x14, 0, 0}, {0x3036, 0x38, 0, 0},
 263         {0x3037, 0x13, 0, 0}, {0x3630, 0x36, 0, 0},
 264         {0x3631, 0x0e, 0, 0}, {0x3632, 0xe2, 0, 0}, {0x3633, 0x12, 0, 0},
 265         {0x3621, 0xe0, 0, 0}, {0x3704, 0xa0, 0, 0}, {0x3703, 0x5a, 0, 0},
 266         {0x3715, 0x78, 0, 0}, {0x3717, 0x01, 0, 0}, {0x370b, 0x60, 0, 0},
 267         {0x3705, 0x1a, 0, 0}, {0x3905, 0x02, 0, 0}, {0x3906, 0x10, 0, 0},
 268         {0x3901, 0x0a, 0, 0}, {0x3731, 0x12, 0, 0}, {0x3600, 0x08, 0, 0},
 269         {0x3601, 0x33, 0, 0}, {0x302d, 0x60, 0, 0}, {0x3620, 0x52, 0, 0},
 270         {0x371b, 0x20, 0, 0}, {0x471c, 0x50, 0, 0}, {0x3a13, 0x43, 0, 0},
 271         {0x3a18, 0x00, 0, 0}, {0x3a19, 0xf8, 0, 0}, {0x3635, 0x13, 0, 0},
 272         {0x3636, 0x03, 0, 0}, {0x3634, 0x40, 0, 0}, {0x3622, 0x01, 0, 0},
 273         {0x3c01, 0xa4, 0, 0}, {0x3c04, 0x28, 0, 0}, {0x3c05, 0x98, 0, 0},
 274         {0x3c06, 0x00, 0, 0}, {0x3c07, 0x08, 0, 0}, {0x3c08, 0x00, 0, 0},
 275         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 276         {0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
 277         {0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
 278         {0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
 279         {0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
 280         {0x3810, 0x00, 0, 0},
 281         {0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
 282         {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
 283         {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
 284         {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
 285         {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
 286         {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
 287         {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x3000, 0x00, 0, 0},
 288         {0x3002, 0x1c, 0, 0}, {0x3004, 0xff, 0, 0}, {0x3006, 0xc3, 0, 0},
 289         {0x300e, 0x45, 0, 0}, {0x302e, 0x08, 0, 0}, {0x4300, 0x3f, 0, 0},
 290         {0x501f, 0x00, 0, 0}, {0x4713, 0x03, 0, 0}, {0x4407, 0x04, 0, 0},
 291         {0x440e, 0x00, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
 292         {0x4837, 0x0a, 0, 0}, {0x4800, 0x04, 0, 0}, {0x3824, 0x02, 0, 0},
 293         {0x5000, 0xa7, 0, 0}, {0x5001, 0xa3, 0, 0}, {0x5180, 0xff, 0, 0},
 294         {0x5181, 0xf2, 0, 0}, {0x5182, 0x00, 0, 0}, {0x5183, 0x14, 0, 0},
 295         {0x5184, 0x25, 0, 0}, {0x5185, 0x24, 0, 0}, {0x5186, 0x09, 0, 0},
 296         {0x5187, 0x09, 0, 0}, {0x5188, 0x09, 0, 0}, {0x5189, 0x88, 0, 0},
 297         {0x518a, 0x54, 0, 0}, {0x518b, 0xee, 0, 0}, {0x518c, 0xb2, 0, 0},
 298         {0x518d, 0x50, 0, 0}, {0x518e, 0x34, 0, 0}, {0x518f, 0x6b, 0, 0},
 299         {0x5190, 0x46, 0, 0}, {0x5191, 0xf8, 0, 0}, {0x5192, 0x04, 0, 0},
 300         {0x5193, 0x70, 0, 0}, {0x5194, 0xf0, 0, 0}, {0x5195, 0xf0, 0, 0},
 301         {0x5196, 0x03, 0, 0}, {0x5197, 0x01, 0, 0}, {0x5198, 0x04, 0, 0},
 302         {0x5199, 0x6c, 0, 0}, {0x519a, 0x04, 0, 0}, {0x519b, 0x00, 0, 0},
 303         {0x519c, 0x09, 0, 0}, {0x519d, 0x2b, 0, 0}, {0x519e, 0x38, 0, 0},
 304         {0x5381, 0x1e, 0, 0}, {0x5382, 0x5b, 0, 0}, {0x5383, 0x08, 0, 0},
 305         {0x5384, 0x0a, 0, 0}, {0x5385, 0x7e, 0, 0}, {0x5386, 0x88, 0, 0},
 306         {0x5387, 0x7c, 0, 0}, {0x5388, 0x6c, 0, 0}, {0x5389, 0x10, 0, 0},
 307         {0x538a, 0x01, 0, 0}, {0x538b, 0x98, 0, 0}, {0x5300, 0x08, 0, 0},
 308         {0x5301, 0x30, 0, 0}, {0x5302, 0x10, 0, 0}, {0x5303, 0x00, 0, 0},
 309         {0x5304, 0x08, 0, 0}, {0x5305, 0x30, 0, 0}, {0x5306, 0x08, 0, 0},
 310         {0x5307, 0x16, 0, 0}, {0x5309, 0x08, 0, 0}, {0x530a, 0x30, 0, 0},
 311         {0x530b, 0x04, 0, 0}, {0x530c, 0x06, 0, 0}, {0x5480, 0x01, 0, 0},
 312         {0x5481, 0x08, 0, 0}, {0x5482, 0x14, 0, 0}, {0x5483, 0x28, 0, 0},
 313         {0x5484, 0x51, 0, 0}, {0x5485, 0x65, 0, 0}, {0x5486, 0x71, 0, 0},
 314         {0x5487, 0x7d, 0, 0}, {0x5488, 0x87, 0, 0}, {0x5489, 0x91, 0, 0},
 315         {0x548a, 0x9a, 0, 0}, {0x548b, 0xaa, 0, 0}, {0x548c, 0xb8, 0, 0},
 316         {0x548d, 0xcd, 0, 0}, {0x548e, 0xdd, 0, 0}, {0x548f, 0xea, 0, 0},
 317         {0x5490, 0x1d, 0, 0}, {0x5580, 0x02, 0, 0}, {0x5583, 0x40, 0, 0},
 318         {0x5584, 0x10, 0, 0}, {0x5589, 0x10, 0, 0}, {0x558a, 0x00, 0, 0},
 319         {0x558b, 0xf8, 0, 0}, {0x5800, 0x23, 0, 0}, {0x5801, 0x14, 0, 0},
 320         {0x5802, 0x0f, 0, 0}, {0x5803, 0x0f, 0, 0}, {0x5804, 0x12, 0, 0},
 321         {0x5805, 0x26, 0, 0}, {0x5806, 0x0c, 0, 0}, {0x5807, 0x08, 0, 0},
 322         {0x5808, 0x05, 0, 0}, {0x5809, 0x05, 0, 0}, {0x580a, 0x08, 0, 0},
 323         {0x580b, 0x0d, 0, 0}, {0x580c, 0x08, 0, 0}, {0x580d, 0x03, 0, 0},
 324         {0x580e, 0x00, 0, 0}, {0x580f, 0x00, 0, 0}, {0x5810, 0x03, 0, 0},
 325         {0x5811, 0x09, 0, 0}, {0x5812, 0x07, 0, 0}, {0x5813, 0x03, 0, 0},
 326         {0x5814, 0x00, 0, 0}, {0x5815, 0x01, 0, 0}, {0x5816, 0x03, 0, 0},
 327         {0x5817, 0x08, 0, 0}, {0x5818, 0x0d, 0, 0}, {0x5819, 0x08, 0, 0},
 328         {0x581a, 0x05, 0, 0}, {0x581b, 0x06, 0, 0}, {0x581c, 0x08, 0, 0},
 329         {0x581d, 0x0e, 0, 0}, {0x581e, 0x29, 0, 0}, {0x581f, 0x17, 0, 0},
 330         {0x5820, 0x11, 0, 0}, {0x5821, 0x11, 0, 0}, {0x5822, 0x15, 0, 0},
 331         {0x5823, 0x28, 0, 0}, {0x5824, 0x46, 0, 0}, {0x5825, 0x26, 0, 0},
 332         {0x5826, 0x08, 0, 0}, {0x5827, 0x26, 0, 0}, {0x5828, 0x64, 0, 0},
 333         {0x5829, 0x26, 0, 0}, {0x582a, 0x24, 0, 0}, {0x582b, 0x22, 0, 0},
 334         {0x582c, 0x24, 0, 0}, {0x582d, 0x24, 0, 0}, {0x582e, 0x06, 0, 0},
 335         {0x582f, 0x22, 0, 0}, {0x5830, 0x40, 0, 0}, {0x5831, 0x42, 0, 0},
 336         {0x5832, 0x24, 0, 0}, {0x5833, 0x26, 0, 0}, {0x5834, 0x24, 0, 0},
 337         {0x5835, 0x22, 0, 0}, {0x5836, 0x22, 0, 0}, {0x5837, 0x26, 0, 0},
 338         {0x5838, 0x44, 0, 0}, {0x5839, 0x24, 0, 0}, {0x583a, 0x26, 0, 0},
 339         {0x583b, 0x28, 0, 0}, {0x583c, 0x42, 0, 0}, {0x583d, 0xce, 0, 0},
 340         {0x5025, 0x00, 0, 0}, {0x3a0f, 0x30, 0, 0}, {0x3a10, 0x28, 0, 0},
 341         {0x3a1b, 0x30, 0, 0}, {0x3a1e, 0x26, 0, 0}, {0x3a11, 0x60, 0, 0},
 342         {0x3a1f, 0x14, 0, 0}, {0x3008, 0x02, 0, 0}, {0x3c00, 0x04, 0, 300},
 343 };
 344
 345 static const struct reg_value ov5640_setting_30fps_VGA_640_480[] = {
 346         {0x3035, 0x14, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
 347         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 348         {0x3814, 0x31, 0, 0},
 349         {0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
 350         {0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
 351         {0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
 352         {0x3810, 0x00, 0, 0},
 353         {0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
 354         {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
 355         {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
 356         {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x0e, 0, 0},
 357         {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
 358         {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
 359         {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
 360         {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
 361         {0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0}, {0x3503, 0x00, 0, 0},
 362 };
 363
 364 static const struct reg_value ov5640_setting_15fps_VGA_640_480[] = {
 365         {0x3035, 0x22, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
 366         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 367         {0x3814, 0x31, 0, 0},
 368         {0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
 369         {0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
 370         {0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
 371         {0x3810, 0x00, 0, 0},
 372         {0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
 373         {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
 374         {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
 375         {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
 376         {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
 377         {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
 378         {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
 379         {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
 380         {0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
 381 };
 382
 383 static const struct reg_value ov5640_setting_30fps_XGA_1024_768[] = {
 384         {0x3035, 0x14, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
 385         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 386         {0x3814, 0x31, 0, 0},
 387         {0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
 388         {0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
 389         {0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
 390         {0x3810, 0x00, 0, 0},
 391         {0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
 392         {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
 393         {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
 394         {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x0e, 0, 0},
 395         {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
 396         {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
 397         {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
 398         {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
 399         {0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0}, {0x3503, 0x00, 0, 0},
 400         {0x3035, 0x12, 0, 0},
 401 };
 402
 403 static const struct reg_value ov5640_setting_15fps_XGA_1024_768[] = {
 404         {0x3035, 0x22, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
 405         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 406         {0x3814, 0x31, 0, 0},
 407         {0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
 408         {0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
 409         {0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
 410         {0x3810, 0x00, 0, 0},
 411         {0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
 412         {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
 413         {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
 414         {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
 415         {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
 416         {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
 417         {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
 418         {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
 419         {0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
 420 };
 421
 422 static const struct reg_value ov5640_setting_30fps_QVGA_320_240[] = {
 423         {0x3035, 0x14, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
 424         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 425         {0x3814, 0x31, 0, 0},
 426         {0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
 427         {0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
 428         {0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
 429         {0x3810, 0x00, 0, 0},
 430         {0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 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}, {0x4713, 0x03, 0, 0},
 437         {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
 438         {0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
 439 };
 440
 441 static const struct reg_value ov5640_setting_15fps_QVGA_320_240[] = {
 442         {0x3035, 0x22, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
 443         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 444         {0x3814, 0x31, 0, 0},
 445         {0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
 446         {0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
 447         {0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
 448         {0x3810, 0x00, 0, 0},
 449         {0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
 450         {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
 451         {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
 452         {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
 453         {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
 454         {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
 455         {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
 456         {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
 457         {0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
 458 };
 459
 460 static const struct reg_value ov5640_setting_30fps_QCIF_176_144[] = {
 461         {0x3035, 0x14, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
 462         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 463         {0x3814, 0x31, 0, 0},
 464         {0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
 465         {0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
 466         {0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
 467         {0x3810, 0x00, 0, 0},
 468         {0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
 469         {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
 470         {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
 471         {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
 472         {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
 473         {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
 474         {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
 475         {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
 476         {0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
 477 };
 478
 479 static const struct reg_value ov5640_setting_15fps_QCIF_176_144[] = {
 480         {0x3035, 0x22, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
 481         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 482         {0x3814, 0x31, 0, 0},
 483         {0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
 484         {0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
 485         {0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
 486         {0x3810, 0x00, 0, 0},
 487         {0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
 488         {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
 489         {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
 490         {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
 491         {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
 492         {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
 493         {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
 494         {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
 495         {0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
 496 };
 497
 498 static const struct reg_value ov5640_setting_30fps_NTSC_720_480[] = {
 499         {0x3035, 0x12, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
 500         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 501         {0x3814, 0x31, 0, 0},
 502         {0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
 503         {0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
 504         {0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
 505         {0x3810, 0x00, 0, 0},
 506         {0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x3c, 0, 0},
 507         {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
 508         {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
 509         {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
 510         {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
 511         {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
 512         {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
 513         {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
 514         {0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
 515 };
 516
 517 static const struct reg_value ov5640_setting_15fps_NTSC_720_480[] = {
 518         {0x3035, 0x22, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
 519         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 520         {0x3814, 0x31, 0, 0},
 521         {0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
 522         {0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
 523         {0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
 524         {0x3810, 0x00, 0, 0},
 525         {0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x3c, 0, 0},
 526         {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
 527         {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
 528         {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
 529         {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
 530         {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
 531         {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
 532         {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
 533         {0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
 534 };
 535
 536 static const struct reg_value ov5640_setting_30fps_PAL_720_576[] = {
 537         {0x3035, 0x12, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
 538         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 539         {0x3814, 0x31, 0, 0},
 540         {0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
 541         {0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
 542         {0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
 543         {0x3810, 0x00, 0, 0},
 544         {0x3811, 0x38, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
 545         {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
 546         {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
 547         {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
 548         {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
 549         {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
 550         {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
 551         {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
 552         {0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
 553 };
 554
 555 static const struct reg_value ov5640_setting_15fps_PAL_720_576[] = {
 556         {0x3035, 0x22, 0, 0}, {0x3036, 0x38, 0, 0}, {0x3c07, 0x08, 0, 0},
 557         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 558         {0x3814, 0x31, 0, 0},
 559         {0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
 560         {0x3802, 0x00, 0, 0}, {0x3803, 0x04, 0, 0}, {0x3804, 0x0a, 0, 0},
 561         {0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9b, 0, 0},
 562         {0x3810, 0x00, 0, 0},
 563         {0x3811, 0x38, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x06, 0, 0},
 564         {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
 565         {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
 566         {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
 567         {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
 568         {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
 569         {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x03, 0, 0},
 570         {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
 571         {0x3824, 0x02, 0, 0}, {0x5001, 0xa3, 0, 0},
 572 };
 573
 574 static const struct reg_value ov5640_setting_30fps_720P_1280_720[] = {
 575         {0x3008, 0x42, 0, 0},
 576         {0x3035, 0x21, 0, 0}, {0x3036, 0x54, 0, 0}, {0x3c07, 0x07, 0, 0},
 577         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 578         {0x3814, 0x31, 0, 0},
 579         {0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
 580         {0x3802, 0x00, 0, 0}, {0x3803, 0xfa, 0, 0}, {0x3804, 0x0a, 0, 0},
 581         {0x3805, 0x3f, 0, 0}, {0x3806, 0x06, 0, 0}, {0x3807, 0xa9, 0, 0},
 582         {0x3810, 0x00, 0, 0},
 583         {0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x04, 0, 0},
 584         {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
 585         {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x02, 0, 0},
 586         {0x3a03, 0xe4, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0xbc, 0, 0},
 587         {0x3a0a, 0x01, 0, 0}, {0x3a0b, 0x72, 0, 0}, {0x3a0e, 0x01, 0, 0},
 588         {0x3a0d, 0x02, 0, 0}, {0x3a14, 0x02, 0, 0}, {0x3a15, 0xe4, 0, 0},
 589         {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x02, 0, 0},
 590         {0x4407, 0x04, 0, 0}, {0x460b, 0x37, 0, 0}, {0x460c, 0x20, 0, 0},
 591         {0x3824, 0x04, 0, 0}, {0x5001, 0x83, 0, 0}, {0x4005, 0x1a, 0, 0},
 592         {0x3008, 0x02, 0, 0}, {0x3503, 0,    0, 0},
 593 };
 594
 595 static const struct reg_value ov5640_setting_15fps_720P_1280_720[] = {
 596         {0x3035, 0x41, 0, 0}, {0x3036, 0x54, 0, 0}, {0x3c07, 0x07, 0, 0},
 597         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 598         {0x3814, 0x31, 0, 0},
 599         {0x3815, 0x31, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
 600         {0x3802, 0x00, 0, 0}, {0x3803, 0xfa, 0, 0}, {0x3804, 0x0a, 0, 0},
 601         {0x3805, 0x3f, 0, 0}, {0x3806, 0x06, 0, 0}, {0x3807, 0xa9, 0, 0},
 602         {0x3810, 0x00, 0, 0},
 603         {0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x04, 0, 0},
 604         {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
 605         {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x02, 0, 0},
 606         {0x3a03, 0xe4, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0xbc, 0, 0},
 607         {0x3a0a, 0x01, 0, 0}, {0x3a0b, 0x72, 0, 0}, {0x3a0e, 0x01, 0, 0},
 608         {0x3a0d, 0x02, 0, 0}, {0x3a14, 0x02, 0, 0}, {0x3a15, 0xe4, 0, 0},
 609         {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x4713, 0x02, 0, 0},
 610         {0x4407, 0x04, 0, 0}, {0x460b, 0x37, 0, 0}, {0x460c, 0x20, 0, 0},
 611         {0x3824, 0x04, 0, 0}, {0x5001, 0x83, 0, 0},
 612 };
 613
 614 static const struct reg_value ov5640_setting_30fps_1080P_1920_1080[] = {
 615         {0x3008, 0x42, 0, 0},
 616         {0x3035, 0x21, 0, 0}, {0x3036, 0x54, 0, 0}, {0x3c07, 0x08, 0, 0},
 617         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 618         {0x3814, 0x11, 0, 0},
 619         {0x3815, 0x11, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
 620         {0x3802, 0x00, 0, 0}, {0x3803, 0x00, 0, 0}, {0x3804, 0x0a, 0, 0},
 621         {0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9f, 0, 0},
 622         {0x3810, 0x00, 0, 0},
 623         {0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x04, 0, 0},
 624         {0x3618, 0x04, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x21, 0, 0},
 625         {0x3709, 0x12, 0, 0}, {0x370c, 0x00, 0, 0}, {0x3a02, 0x03, 0, 0},
 626         {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
 627         {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
 628         {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
 629         {0x4001, 0x02, 0, 0}, {0x4004, 0x06, 0, 0}, {0x4713, 0x03, 0, 0},
 630         {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
 631         {0x3824, 0x02, 0, 0}, {0x5001, 0x83, 0, 0}, {0x3035, 0x11, 0, 0},
 632         {0x3036, 0x54, 0, 0}, {0x3c07, 0x07, 0, 0}, {0x3c08, 0x00, 0, 0},
 633         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 634         {0x3800, 0x01, 0, 0}, {0x3801, 0x50, 0, 0}, {0x3802, 0x01, 0, 0},
 635         {0x3803, 0xb2, 0, 0}, {0x3804, 0x08, 0, 0}, {0x3805, 0xef, 0, 0},
 636         {0x3806, 0x05, 0, 0}, {0x3807, 0xf1, 0, 0},
 637         {0x3612, 0x2b, 0, 0}, {0x3708, 0x64, 0, 0},
 638         {0x3a02, 0x04, 0, 0}, {0x3a03, 0x60, 0, 0}, {0x3a08, 0x01, 0, 0},
 639         {0x3a09, 0x50, 0, 0}, {0x3a0a, 0x01, 0, 0}, {0x3a0b, 0x18, 0, 0},
 640         {0x3a0e, 0x03, 0, 0}, {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x04, 0, 0},
 641         {0x3a15, 0x60, 0, 0}, {0x4713, 0x02, 0, 0}, {0x4407, 0x04, 0, 0},
 642         {0x460b, 0x37, 0, 0}, {0x460c, 0x20, 0, 0}, {0x3824, 0x04, 0, 0},
 643         {0x4005, 0x1a, 0, 0}, {0x3008, 0x02, 0, 0},
 644         {0x3503, 0, 0, 0},
 645 };
 646
 647 static const struct reg_value ov5640_setting_15fps_1080P_1920_1080[] = {
 648         {0x3008, 0x42, 0, 0},
 649         {0x3035, 0x21, 0, 0}, {0x3036, 0x54, 0, 0}, {0x3c07, 0x08, 0, 0},
 650         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 651         {0x3814, 0x11, 0, 0},
 652         {0x3815, 0x11, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
 653         {0x3802, 0x00, 0, 0}, {0x3803, 0x00, 0, 0}, {0x3804, 0x0a, 0, 0},
 654         {0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9f, 0, 0},
 655         {0x3810, 0x00, 0, 0},
 656         {0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x04, 0, 0},
 657         {0x3618, 0x04, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x21, 0, 0},
 658         {0x3709, 0x12, 0, 0}, {0x370c, 0x00, 0, 0}, {0x3a02, 0x03, 0, 0},
 659         {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
 660         {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
 661         {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
 662         {0x4001, 0x02, 0, 0}, {0x4004, 0x06, 0, 0}, {0x4713, 0x03, 0, 0},
 663         {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
 664         {0x3824, 0x02, 0, 0}, {0x5001, 0x83, 0, 0}, {0x3035, 0x21, 0, 0},
 665         {0x3036, 0x54, 0, 1}, {0x3c07, 0x07, 0, 0}, {0x3c08, 0x00, 0, 0},
 666         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 667         {0x3800, 0x01, 0, 0}, {0x3801, 0x50, 0, 0}, {0x3802, 0x01, 0, 0},
 668         {0x3803, 0xb2, 0, 0}, {0x3804, 0x08, 0, 0}, {0x3805, 0xef, 0, 0},
 669         {0x3806, 0x05, 0, 0}, {0x3807, 0xf1, 0, 0},
 670         {0x3612, 0x2b, 0, 0}, {0x3708, 0x64, 0, 0},
 671         {0x3a02, 0x04, 0, 0}, {0x3a03, 0x60, 0, 0}, {0x3a08, 0x01, 0, 0},
 672         {0x3a09, 0x50, 0, 0}, {0x3a0a, 0x01, 0, 0}, {0x3a0b, 0x18, 0, 0},
 673         {0x3a0e, 0x03, 0, 0}, {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x04, 0, 0},
 674         {0x3a15, 0x60, 0, 0}, {0x4713, 0x02, 0, 0}, {0x4407, 0x04, 0, 0},
 675         {0x460b, 0x37, 0, 0}, {0x460c, 0x20, 0, 0}, {0x3824, 0x04, 0, 0},
 676         {0x4005, 0x1a, 0, 0}, {0x3008, 0x02, 0, 0}, {0x3503, 0, 0, 0},
 677 };
 678
 679 static const struct reg_value ov5640_setting_15fps_QSXGA_2592_1944[] = {
 680         {0x3035, 0x21, 0, 0}, {0x3036, 0x54, 0, 0}, {0x3c07, 0x08, 0, 0},
 681         {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
 682         {0x3814, 0x11, 0, 0},
 683         {0x3815, 0x11, 0, 0}, {0x3800, 0x00, 0, 0}, {0x3801, 0x00, 0, 0},
 684         {0x3802, 0x00, 0, 0}, {0x3803, 0x00, 0, 0}, {0x3804, 0x0a, 0, 0},
 685         {0x3805, 0x3f, 0, 0}, {0x3806, 0x07, 0, 0}, {0x3807, 0x9f, 0, 0},
 686         {0x3810, 0x00, 0, 0},
 687         {0x3811, 0x10, 0, 0}, {0x3812, 0x00, 0, 0}, {0x3813, 0x04, 0, 0},
 688         {0x3618, 0x04, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x21, 0, 0},
 689         {0x3709, 0x12, 0, 0}, {0x370c, 0x00, 0, 0}, {0x3a02, 0x03, 0, 0},
 690         {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
 691         {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
 692         {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
 693         {0x4001, 0x02, 0, 0}, {0x4004, 0x06, 0, 0}, {0x4713, 0x03, 0, 0},
 694         {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
 695         {0x3824, 0x02, 0, 0}, {0x5001, 0x83, 0, 70},
 696 };
 697
 698 /* power-on sensor init reg table */
 699 static const struct ov5640_mode_info ov5640_mode_init_data = {
 700         0, SUBSAMPLING, 640, 1896, 480, 984,
 701         ov5640_init_setting_30fps_VGA,
 702         ARRAY_SIZE(ov5640_init_setting_30fps_VGA),
 703 };
 704
 705 static const struct ov5640_mode_info
 706 ov5640_mode_data[OV5640_NUM_FRAMERATES][OV5640_NUM_MODES] = {
 707         {
 708                 {OV5640_MODE_QCIF_176_144, SUBSAMPLING,
 709                  176, 1896, 144, 984,
 710                  ov5640_setting_15fps_QCIF_176_144,
 711                  ARRAY_SIZE(ov5640_setting_15fps_QCIF_176_144)},
 712                 {OV5640_MODE_QVGA_320_240, SUBSAMPLING,
 713                  320, 1896, 240, 984,
 714                  ov5640_setting_15fps_QVGA_320_240,
 715                  ARRAY_SIZE(ov5640_setting_15fps_QVGA_320_240)},
 716                 {OV5640_MODE_VGA_640_480, SUBSAMPLING,
 717                  640, 1896, 480, 1080,
 718                  ov5640_setting_15fps_VGA_640_480,
 719                  ARRAY_SIZE(ov5640_setting_15fps_VGA_640_480)},
 720                 {OV5640_MODE_NTSC_720_480, SUBSAMPLING,
 721                  720, 1896, 480, 984,
 722                  ov5640_setting_15fps_NTSC_720_480,
 723                  ARRAY_SIZE(ov5640_setting_15fps_NTSC_720_480)},
 724                 {OV5640_MODE_PAL_720_576, SUBSAMPLING,
 725                  720, 1896, 576, 984,
 726                  ov5640_setting_15fps_PAL_720_576,
 727                  ARRAY_SIZE(ov5640_setting_15fps_PAL_720_576)},
 728                 {OV5640_MODE_XGA_1024_768, SUBSAMPLING,
 729                  1024, 1896, 768, 1080,
 730                  ov5640_setting_15fps_XGA_1024_768,
 731                  ARRAY_SIZE(ov5640_setting_15fps_XGA_1024_768)},
 732                 {OV5640_MODE_720P_1280_720, SUBSAMPLING,
 733                  1280, 1892, 720, 740,
 734                  ov5640_setting_15fps_720P_1280_720,
 735                  ARRAY_SIZE(ov5640_setting_15fps_720P_1280_720)},
 736                 {OV5640_MODE_1080P_1920_1080, SCALING,
 737                  1920, 2500, 1080, 1120,
 738                  ov5640_setting_15fps_1080P_1920_1080,
 739                  ARRAY_SIZE(ov5640_setting_15fps_1080P_1920_1080)},
 740                 {OV5640_MODE_QSXGA_2592_1944, SCALING,
 741                  2592, 2844, 1944, 1968,
 742                  ov5640_setting_15fps_QSXGA_2592_1944,
 743                  ARRAY_SIZE(ov5640_setting_15fps_QSXGA_2592_1944)},
 744         }, {
 745                 {OV5640_MODE_QCIF_176_144, SUBSAMPLING,
 746                  176, 1896, 144, 984,
 747                  ov5640_setting_30fps_QCIF_176_144,
 748                  ARRAY_SIZE(ov5640_setting_30fps_QCIF_176_144)},
 749                 {OV5640_MODE_QVGA_320_240, SUBSAMPLING,
 750                  320, 1896, 240, 984,
 751                  ov5640_setting_30fps_QVGA_320_240,
 752                  ARRAY_SIZE(ov5640_setting_30fps_QVGA_320_240)},
 753                 {OV5640_MODE_VGA_640_480, SUBSAMPLING,
 754                  640, 1896, 480, 1080,
 755                  ov5640_setting_30fps_VGA_640_480,
 756                  ARRAY_SIZE(ov5640_setting_30fps_VGA_640_480)},
 757                 {OV5640_MODE_NTSC_720_480, SUBSAMPLING,
 758                  720, 1896, 480, 984,
 759                  ov5640_setting_30fps_NTSC_720_480,
 760                  ARRAY_SIZE(ov5640_setting_30fps_NTSC_720_480)},
 761                 {OV5640_MODE_PAL_720_576, SUBSAMPLING,
 762                  720, 1896, 576, 984,
 763                  ov5640_setting_30fps_PAL_720_576,
 764                  ARRAY_SIZE(ov5640_setting_30fps_PAL_720_576)},
 765                 {OV5640_MODE_XGA_1024_768, SUBSAMPLING,
 766                  1024, 1896, 768, 1080,
 767                  ov5640_setting_30fps_XGA_1024_768,
 768                  ARRAY_SIZE(ov5640_setting_30fps_XGA_1024_768)},
 769                 {OV5640_MODE_720P_1280_720, SUBSAMPLING,
 770                  1280, 1892, 720, 740,
 771                  ov5640_setting_30fps_720P_1280_720,
 772                  ARRAY_SIZE(ov5640_setting_30fps_720P_1280_720)},
 773                 {OV5640_MODE_1080P_1920_1080, SCALING,
 774                  1920, 2500, 1080, 1120,
 775                  ov5640_setting_30fps_1080P_1920_1080,
 776                  ARRAY_SIZE(ov5640_setting_30fps_1080P_1920_1080)},
 777                 {OV5640_MODE_QSXGA_2592_1944, -1, 0, 0, 0, 0, NULL, 0},
 778         },
 779 };
 780
 781 static int ov5640_init_slave_id(struct ov5640_dev *sensor)
 782 {
 783         struct i2c_client *client = sensor->i2c_client;
 784         struct i2c_msg msg;
 785         u8 buf[3];
 786         int ret;
 787
 788         if (client->addr == OV5640_DEFAULT_SLAVE_ID)
 789                 return 0;
 790
 791         buf[0] = OV5640_REG_SLAVE_ID >> 8;
 792         buf[1] = OV5640_REG_SLAVE_ID & 0xff;
 793         buf[2] = client->addr << 1;
 794
 795         msg.addr = OV5640_DEFAULT_SLAVE_ID;
 796         msg.flags = 0;
 797         msg.buf = buf;
 798         msg.len = sizeof(buf);
 799
 800         ret = i2c_transfer(client->adapter, &msg, 1);
 801         if (ret < 0) {
 802                 dev_err(&client->dev, "%s: failed with %d\n", __func__, ret);
 803                 return ret;
 804         }
 805
 806         return 0;
 807 }
 808
 809 static int ov5640_write_reg(struct ov5640_dev *sensor, u16 reg, u8 val)
 810 {
 811         struct i2c_client *client = sensor->i2c_client;
 812         struct i2c_msg msg;
 813         u8 buf[3];
 814         int ret;
 815
 816         buf[0] = reg >> 8;
 817         buf[1] = reg & 0xff;
 818         buf[2] = val;
 819
 820         msg.addr = client->addr;
 821         msg.flags = client->flags;
 822         msg.buf = buf;
 823         msg.len = sizeof(buf);
 824
 825         ret = i2c_transfer(client->adapter, &msg, 1);
 826         if (ret < 0) {
 827                 dev_err(&client->dev, "%s: error: reg=%x, val=%x\n",
 828                         __func__, reg, val);
 829                 return ret;
 830         }
 831
 832         return 0;
 833 }
 834
 835 static int ov5640_read_reg(struct ov5640_dev *sensor, u16 reg, u8 *val)
 836 {
 837         struct i2c_client *client = sensor->i2c_client;
 838         struct i2c_msg msg[2];
 839         u8 buf[2];
 840         int ret;
 841
 842         buf[0] = reg >> 8;
 843         buf[1] = reg & 0xff;
 844
 845         msg[0].addr = client->addr;
 846         msg[0].flags = client->flags;
 847         msg[0].buf = buf;
 848         msg[0].len = sizeof(buf);
 849
 850         msg[1].addr = client->addr;
 851         msg[1].flags = client->flags | I2C_M_RD;
 852         msg[1].buf = buf;
 853         msg[1].len = 1;
 854
 855         ret = i2c_transfer(client->adapter, msg, 2);
 856         if (ret < 0) {
 857                 dev_err(&client->dev, "%s: error: reg=%x\n",
 858                         __func__, reg);
 859                 return ret;
 860         }
 861
 862         *val = buf[0];
 863         return 0;
 864 }
 865
 866 static int ov5640_read_reg16(struct ov5640_dev *sensor, u16 reg, u16 *val)
 867 {
 868         u8 hi, lo;
 869         int ret;
 870
 871         ret = ov5640_read_reg(sensor, reg, &hi);
 872         if (ret)
 873                 return ret;
 874         ret = ov5640_read_reg(sensor, reg + 1, &lo);
 875         if (ret)
 876                 return ret;
 877
 878         *val = ((u16)hi << 8) | (u16)lo;
 879         return 0;
 880 }
 881
 882 static int ov5640_write_reg16(struct ov5640_dev *sensor, u16 reg, u16 val)
 883 {
 884         int ret;
 885
 886         ret = ov5640_write_reg(sensor, reg, val >> 8);
 887         if (ret)
 888                 return ret;
 889
 890         return ov5640_write_reg(sensor, reg + 1, val & 0xff);
 891 }
 892
 893 static int ov5640_mod_reg(struct ov5640_dev *sensor, u16 reg,
 894                           u8 mask, u8 val)
 895 {
 896         u8 readval;
 897         int ret;
 898
 899         ret = ov5640_read_reg(sensor, reg, &readval);
 900         if (ret)
 901                 return ret;
 902
 903         readval &= ~mask;
 904         val &= mask;
 905         val |= readval;
 906
 907         return ov5640_write_reg(sensor, reg, val);
 908 }
 909
 910 /* download ov5640 settings to sensor through i2c */
 911 static int ov5640_load_regs(struct ov5640_dev *sensor,
 912                             const struct ov5640_mode_info *mode)
 913 {
 914         const struct reg_value *regs = mode->reg_data;
 915         unsigned int i;
 916         u32 delay_ms;
 917         u16 reg_addr;
 918         u8 mask, val;
 919         int ret = 0;
 920
 921         for (i = 0; i < mode->reg_data_size; ++i, ++regs) {
 922                 delay_ms = regs->delay_ms;
 923                 reg_addr = regs->reg_addr;
 924                 val = regs->val;
 925                 mask = regs->mask;
 926
 927                 if (mask)
 928                         ret = ov5640_mod_reg(sensor, reg_addr, mask, val);
 929                 else
 930                         ret = ov5640_write_reg(sensor, reg_addr, val);
 931                 if (ret)
 932                         break;
 933
 934                 if (delay_ms)
 935                         usleep_range(1000 * delay_ms, 1000 * delay_ms + 100);
 936         }
 937
 938         return ret;
 939 }
 940
 941 /* read exposure, in number of line periods */
 942 static int ov5640_get_exposure(struct ov5640_dev *sensor)
 943 {
 944         int exp, ret;
 945         u8 temp;
 946
 947         ret = ov5640_read_reg(sensor, OV5640_REG_AEC_PK_EXPOSURE_HI, &temp);
 948         if (ret)
 949                 return ret;
 950         exp = ((int)temp & 0x0f) << 16;
 951         ret = ov5640_read_reg(sensor, OV5640_REG_AEC_PK_EXPOSURE_MED, &temp);
 952         if (ret)
 953                 return ret;
 954         exp |= ((int)temp << 8);
 955         ret = ov5640_read_reg(sensor, OV5640_REG_AEC_PK_EXPOSURE_LO, &temp);
 956         if (ret)
 957                 return ret;
 958         exp |= (int)temp;
 959
 960         return exp >> 4;
 961 }
 962
 963 /* write exposure, given number of line periods */
 964 static int ov5640_set_exposure(struct ov5640_dev *sensor, u32 exposure)
 965 {
 966         int ret;
 967
 968         exposure <<= 4;
 969
 970         ret = ov5640_write_reg(sensor,
 971                                OV5640_REG_AEC_PK_EXPOSURE_LO,
 972                                exposure & 0xff);
 973         if (ret)
 974                 return ret;
 975         ret = ov5640_write_reg(sensor,
 976                                OV5640_REG_AEC_PK_EXPOSURE_MED,
 977                                (exposure >> 8) & 0xff);
 978         if (ret)
 979                 return ret;
 980         return ov5640_write_reg(sensor,
 981                                 OV5640_REG_AEC_PK_EXPOSURE_HI,
 982                                 (exposure >> 16) & 0x0f);
 983 }
 984
 985 static int ov5640_get_gain(struct ov5640_dev *sensor)
 986 {
 987         u16 gain;
 988         int ret;
 989
 990         ret = ov5640_read_reg16(sensor, OV5640_REG_AEC_PK_REAL_GAIN, &gain);
 991         if (ret)
 992                 return ret;
 993
 994         return gain & 0x3ff;
 995 }
 996
 997 static int ov5640_set_stream_dvp(struct ov5640_dev *sensor, bool on)
 998 {
 999         int ret;
1000         unsigned int flags = sensor->ep.bus.parallel.flags;
1001         u8 pclk_pol = 0;
1002         u8 hsync_pol = 0;
1003         u8 vsync_pol = 0;
1004
1005         /*
1006          * Note about parallel port configuration.
1007          *
1008          * When configured in parallel mode, the OV5640 will
1009          * output 10 bits data on DVP data lines [9:0].
1010          * If only 8 bits data are wanted, the 8 bits data lines
1011          * of the camera interface must be physically connected
1012          * on the DVP data lines [9:2].
1013          *
1014          * Control lines polarity can be configured through
1015          * devicetree endpoint control lines properties.
1016          * If no endpoint control lines properties are set,
1017          * polarity will be as below:
1018          * - VSYNC:     active high
1019          * - HREF:      active low
1020          * - PCLK:      active low
1021          */
1022
1023         if (on) {
1024                 /*
1025                  * reset MIPI PCLK/SERCLK divider
1026                  *
1027                  * SC PLL CONTRL1 0
1028                  * - [3..0]:    MIPI PCLK/SERCLK divider
1029                  */
1030                 ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL1, 0x0f, 0);
1031                 if (ret)
1032                         return ret;
1033
1034                 /*
1035                  * configure parallel port control lines polarity
1036                  *
1037                  * POLARITY CTRL0
1038                  * - [5]:       PCLK polarity (0: active low, 1: active high)
1039                  * - [1]:       HREF polarity (0: active low, 1: active high)
1040                  * - [0]:       VSYNC polarity (mismatch here between
1041                  *              datasheet and hardware, 0 is active high
1042                  *              and 1 is active low...)
1043                  */
1044                 if (flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
1045                         pclk_pol = 1;
1046                 if (flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
1047                         hsync_pol = 1;
1048                 if (flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
1049                         vsync_pol = 1;
1050
1051                 ret = ov5640_write_reg(sensor,
1052                                        OV5640_REG_POLARITY_CTRL00,
1053                                        (pclk_pol << 5) |
1054                                        (hsync_pol << 1) |
1055                                        vsync_pol);
1056
1057                 if (ret)
1058                         return ret;
1059         }
1060
1061         /*
1062          * powerdown MIPI TX/RX PHY & disable MIPI
1063          *
1064          * MIPI CONTROL 00
1065          * 4:    PWDN PHY TX
1066          * 3:    PWDN PHY RX
1067          * 2:    MIPI enable
1068          */
1069         ret = ov5640_write_reg(sensor,
1070                                OV5640_REG_IO_MIPI_CTRL00, on ? 0x18 : 0);
1071         if (ret)
1072                 return ret;
1073
1074         /*
1075          * enable VSYNC/HREF/PCLK DVP control lines
1076          * & D[9:6] DVP data lines
1077          *
1078          * PAD OUTPUT ENABLE 01
1079          * - 6:         VSYNC output enable
1080          * - 5:         HREF output enable
1081          * - 4:         PCLK output enable
1082          * - [3:0]:     D[9:6] output enable
1083          */
1084         ret = ov5640_write_reg(sensor,
1085                                OV5640_REG_PAD_OUTPUT_ENABLE01,
1086                                on ? 0x7f : 0);
1087         if (ret)
1088                 return ret;
1089
1090         /*
1091          * enable D[5:0] DVP data lines
1092          *
1093          * PAD OUTPUT ENABLE 02
1094          * - [7:2]:     D[5:0] output enable
1095          */
1096         return ov5640_write_reg(sensor,
1097                                 OV5640_REG_PAD_OUTPUT_ENABLE02,
1098                                 on ? 0xfc : 0);
1099 }
1100
1101 static int ov5640_set_stream_mipi(struct ov5640_dev *sensor, bool on)
1102 {
1103         int ret;
1104
1105         ret = ov5640_mod_reg(sensor, OV5640_REG_MIPI_CTRL00, BIT(5),
1106                              on ? 0 : BIT(5));
1107         if (ret)
1108                 return ret;
1109         ret = ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT00,
1110                                on ? 0x00 : 0x70);
1111         if (ret)
1112                 return ret;
1113
1114         return ov5640_write_reg(sensor, OV5640_REG_FRAME_CTRL01,
1115                                 on ? 0x00 : 0x0f);
1116 }
1117
1118 static int ov5640_get_sysclk(struct ov5640_dev *sensor)
1119 {
1120          /* calculate sysclk */
1121         u32 xvclk = sensor->xclk_freq / 10000;
1122         u32 multiplier, prediv, VCO, sysdiv, pll_rdiv;
1123         u32 sclk_rdiv_map[] = {1, 2, 4, 8};
1124         u32 bit_div2x = 1, sclk_rdiv, sysclk;
1125         u8 temp1, temp2;
1126         int ret;
1127
1128         ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL0, &temp1);
1129         if (ret)
1130                 return ret;
1131         temp2 = temp1 & 0x0f;
1132         if (temp2 == 8 || temp2 == 10)
1133                 bit_div2x = temp2 / 2;
1134
1135         ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL1, &temp1);
1136         if (ret)
1137                 return ret;
1138         sysdiv = temp1 >> 4;
1139         if (sysdiv == 0)
1140                 sysdiv = 16;
1141
1142         ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL2, &temp1);
1143         if (ret)
1144                 return ret;
1145         multiplier = temp1;
1146
1147         ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL3, &temp1);
1148         if (ret)
1149                 return ret;
1150         prediv = temp1 & 0x0f;
1151         pll_rdiv = ((temp1 >> 4) & 0x01) + 1;
1152
1153         ret = ov5640_read_reg(sensor, OV5640_REG_SYS_ROOT_DIVIDER, &temp1);
1154         if (ret)
1155                 return ret;
1156         temp2 = temp1 & 0x03;
1157         sclk_rdiv = sclk_rdiv_map[temp2];
1158
1159         if (!prediv || !sysdiv || !pll_rdiv || !bit_div2x)
1160                 return -EINVAL;
1161
1162         VCO = xvclk * multiplier / prediv;
1163
1164         sysclk = VCO / sysdiv / pll_rdiv * 2 / bit_div2x / sclk_rdiv;
1165
1166         return sysclk;
1167 }
1168
1169 static int ov5640_set_night_mode(struct ov5640_dev *sensor)
1170 {
1171          /* read HTS from register settings */
1172         u8 mode;
1173         int ret;
1174
1175         ret = ov5640_read_reg(sensor, OV5640_REG_AEC_CTRL00, &mode);
1176         if (ret)
1177                 return ret;
1178         mode &= 0xfb;
1179         return ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL00, mode);
1180 }
1181
1182 static int ov5640_get_hts(struct ov5640_dev *sensor)
1183 {
1184         /* read HTS from register settings */
1185         u16 hts;
1186         int ret;
1187
1188         ret = ov5640_read_reg16(sensor, OV5640_REG_TIMING_HTS, &hts);
1189         if (ret)
1190                 return ret;
1191         return hts;
1192 }
1193
1194 static int ov5640_get_vts(struct ov5640_dev *sensor)
1195 {
1196         u16 vts;
1197         int ret;
1198
1199         ret = ov5640_read_reg16(sensor, OV5640_REG_TIMING_VTS, &vts);
1200         if (ret)
1201                 return ret;
1202         return vts;
1203 }
1204
1205 static int ov5640_set_vts(struct ov5640_dev *sensor, int vts)
1206 {
1207         return ov5640_write_reg16(sensor, OV5640_REG_TIMING_VTS, vts);
1208 }
1209
1210 static int ov5640_get_light_freq(struct ov5640_dev *sensor)
1211 {
1212         /* get banding filter value */
1213         int ret, light_freq = 0;
1214         u8 temp, temp1;
1215
1216         ret = ov5640_read_reg(sensor, OV5640_REG_HZ5060_CTRL01, &temp);
1217         if (ret)
1218                 return ret;
1219
1220         if (temp & 0x80) {
1221                 /* manual */
1222                 ret = ov5640_read_reg(sensor, OV5640_REG_HZ5060_CTRL00,
1223                                       &temp1);
1224                 if (ret)
1225                         return ret;
1226                 if (temp1 & 0x04) {
1227                         /* 50Hz */
1228                         light_freq = 50;
1229                 } else {
1230                         /* 60Hz */
1231                         light_freq = 60;
1232                 }
1233         } else {
1234                 /* auto */
1235                 ret = ov5640_read_reg(sensor, OV5640_REG_SIGMADELTA_CTRL0C,
1236                                       &temp1);
1237                 if (ret)
1238                         return ret;
1239
1240                 if (temp1 & 0x01) {
1241                         /* 50Hz */
1242                         light_freq = 50;
1243                 } else {
1244                         /* 60Hz */
1245                 }
1246         }
1247
1248         return light_freq;
1249 }
1250
1251 static int ov5640_set_bandingfilter(struct ov5640_dev *sensor)
1252 {
1253         u32 band_step60, max_band60, band_step50, max_band50, prev_vts;
1254         int ret;
1255
1256         /* read preview PCLK */
1257         ret = ov5640_get_sysclk(sensor);
1258         if (ret < 0)
1259                 return ret;
1260         if (ret == 0)
1261                 return -EINVAL;
1262         sensor->prev_sysclk = ret;
1263         /* read preview HTS */
1264         ret = ov5640_get_hts(sensor);
1265         if (ret < 0)
1266                 return ret;
1267         if (ret == 0)
1268                 return -EINVAL;
1269         sensor->prev_hts = ret;
1270
1271         /* read preview VTS */
1272         ret = ov5640_get_vts(sensor);
1273         if (ret < 0)
1274                 return ret;
1275         prev_vts = ret;
1276
1277         /* calculate banding filter */
1278         /* 60Hz */
1279         band_step60 = sensor->prev_sysclk * 100 / sensor->prev_hts * 100 / 120;
1280         ret = ov5640_write_reg16(sensor, OV5640_REG_AEC_B60_STEP, band_step60);
1281         if (ret)
1282                 return ret;
1283         if (!band_step60)
1284                 return -EINVAL;
1285         max_band60 = (int)((prev_vts - 4) / band_step60);
1286         ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL0D, max_band60);
1287         if (ret)
1288                 return ret;
1289
1290         /* 50Hz */
1291         band_step50 = sensor->prev_sysclk * 100 / sensor->prev_hts;
1292         ret = ov5640_write_reg16(sensor, OV5640_REG_AEC_B50_STEP, band_step50);
1293         if (ret)
1294                 return ret;
1295         if (!band_step50)
1296                 return -EINVAL;
1297         max_band50 = (int)((prev_vts - 4) / band_step50);
1298         return ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL0E, max_band50);
1299 }
1300
1301 static int ov5640_set_ae_target(struct ov5640_dev *sensor, int target)
1302 {
1303         /* stable in high */
1304         u32 fast_high, fast_low;
1305         int ret;
1306
1307         sensor->ae_low = target * 23 / 25;      /* 0.92 */
1308         sensor->ae_high = target * 27 / 25;     /* 1.08 */
1309
1310         fast_high = sensor->ae_high << 1;
1311         if (fast_high > 255)
1312                 fast_high = 255;
1313
1314         fast_low = sensor->ae_low >> 1;
1315
1316         ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL0F, sensor->ae_high);
1317         if (ret)
1318                 return ret;
1319         ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL10, sensor->ae_low);
1320         if (ret)
1321                 return ret;
1322         ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL1B, sensor->ae_high);
1323         if (ret)
1324                 return ret;
1325         ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL1E, sensor->ae_low);
1326         if (ret)
1327                 return ret;
1328         ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL11, fast_high);
1329         if (ret)
1330                 return ret;
1331         return ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL1F, fast_low);
1332 }
1333
1334 static int ov5640_binning_on(struct ov5640_dev *sensor)
1335 {
1336         u8 temp;
1337         int ret;
1338
1339         ret = ov5640_read_reg(sensor, OV5640_REG_TIMING_TC_REG21, &temp);
1340         if (ret)
1341                 return ret;
1342         temp &= 0xfe;
1343         return temp ? 1 : 0;
1344 }
1345
1346 static int ov5640_set_binning(struct ov5640_dev *sensor, bool enable)
1347 {
1348         int ret;
1349
1350         /*
1351          * TIMING TC REG21:
1352          * - [0]:       Horizontal binning enable
1353          */
1354         ret = ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG21,
1355                              BIT(0), enable ? BIT(0) : 0);
1356         if (ret)
1357                 return ret;
1358         /*
1359          * TIMING TC REG20:
1360          * - [0]:       Undocumented, but hardcoded init sequences
1361          *              are always setting REG21/REG20 bit 0 to same value...
1362          */
1363         return ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG20,
1364                               BIT(0), enable ? BIT(0) : 0);
1365 }
1366
1367 static int ov5640_set_virtual_channel(struct ov5640_dev *sensor)
1368 {
1369         struct i2c_client *client = sensor->i2c_client;
1370         u8 temp, channel = virtual_channel;
1371         int ret;
1372
1373         if (channel > 3) {
1374                 dev_err(&client->dev,
1375                         "%s: wrong virtual_channel parameter, expected (0..3), got %d\n",
1376                         __func__, channel);
1377                 return -EINVAL;
1378         }
1379
1380         ret = ov5640_read_reg(sensor, OV5640_REG_DEBUG_MODE, &temp);
1381         if (ret)
1382                 return ret;
1383         temp &= ~(3 << 6);
1384         temp |= (channel << 6);
1385         return ov5640_write_reg(sensor, OV5640_REG_DEBUG_MODE, temp);
1386 }
1387
1388 static int ov5640_set_timings(struct ov5640_dev *sensor,
1389                               const struct ov5640_mode_info *mode)
1390 {
1391         int ret;
1392
1393         ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_DVPHO, mode->hact);
1394         if (ret < 0)
1395                 return ret;
1396
1397         ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_DVPVO, mode->vact);
1398         if (ret < 0)
1399                 return ret;
1400
1401         ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_HTS, mode->htot);
1402         if (ret < 0)
1403                 return ret;
1404
1405         ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_VTS, mode->vtot);
1406         if (ret < 0)
1407                 return ret;
1408
1409         return 0;
1410 }
1411
1412 static const struct ov5640_mode_info *
1413 ov5640_find_mode(struct ov5640_dev *sensor, enum ov5640_frame_rate fr,
1414                  int width, int height, bool nearest)
1415 {
1416         const struct ov5640_mode_info *mode = NULL;
1417         int i;
1418
1419         for (i = OV5640_NUM_MODES - 1; i >= 0; i--) {
1420                 mode = &ov5640_mode_data[fr][i];
1421
1422                 if (!mode->reg_data)
1423                         continue;
1424
1425                 if ((nearest && mode->hact <= width &&
1426                      mode->vact <= height) ||
1427                     (!nearest && mode->hact == width &&
1428                      mode->vact == height))
1429                         break;
1430         }
1431
1432         if (nearest && i < 0)
1433                 mode = &ov5640_mode_data[fr][0];
1434
1435         return mode;
1436 }
1437
1438 /*
1439  * sensor changes between scaling and subsampling, go through
1440  * exposure calculation
1441  */
1442 static int ov5640_set_mode_exposure_calc(struct ov5640_dev *sensor,
1443                                          const struct ov5640_mode_info *mode)
1444 {
1445         u32 prev_shutter, prev_gain16;
1446         u32 cap_shutter, cap_gain16;
1447         u32 cap_sysclk, cap_hts, cap_vts;
1448         u32 light_freq, cap_bandfilt, cap_maxband;
1449         u32 cap_gain16_shutter;
1450         u8 average;
1451         int ret;
1452
1453         if (!mode->reg_data)
1454                 return -EINVAL;
1455
1456         /* read preview shutter */
1457         ret = ov5640_get_exposure(sensor);
1458         if (ret < 0)
1459                 return ret;
1460         prev_shutter = ret;
1461         ret = ov5640_binning_on(sensor);
1462         if (ret < 0)
1463                 return ret;
1464         if (ret && mode->id != OV5640_MODE_720P_1280_720 &&
1465             mode->id != OV5640_MODE_1080P_1920_1080)
1466                 prev_shutter *= 2;
1467
1468         /* read preview gain */
1469         ret = ov5640_get_gain(sensor);
1470         if (ret < 0)
1471                 return ret;
1472         prev_gain16 = ret;
1473
1474         /* get average */
1475         ret = ov5640_read_reg(sensor, OV5640_REG_AVG_READOUT, &average);
1476         if (ret)
1477                 return ret;
1478
1479         /* turn off night mode for capture */
1480         ret = ov5640_set_night_mode(sensor);
1481         if (ret < 0)
1482                 return ret;
1483
1484         /* Write capture setting */
1485         ret = ov5640_load_regs(sensor, mode);
1486         if (ret < 0)
1487                 return ret;
1488
1489         /* read capture VTS */
1490         ret = ov5640_get_vts(sensor);
1491         if (ret < 0)
1492                 return ret;
1493         cap_vts = ret;
1494         ret = ov5640_get_hts(sensor);
1495         if (ret < 0)
1496                 return ret;
1497         if (ret == 0)
1498                 return -EINVAL;
1499         cap_hts = ret;
1500
1501         ret = ov5640_get_sysclk(sensor);
1502         if (ret < 0)
1503                 return ret;
1504         if (ret == 0)
1505                 return -EINVAL;
1506         cap_sysclk = ret;
1507
1508         /* calculate capture banding filter */
1509         ret = ov5640_get_light_freq(sensor);
1510         if (ret < 0)
1511                 return ret;
1512         light_freq = ret;
1513
1514         if (light_freq == 60) {
1515                 /* 60Hz */
1516                 cap_bandfilt = cap_sysclk * 100 / cap_hts * 100 / 120;
1517         } else {
1518                 /* 50Hz */
1519                 cap_bandfilt = cap_sysclk * 100 / cap_hts;
1520         }
1521
1522         if (!sensor->prev_sysclk) {
1523                 ret = ov5640_get_sysclk(sensor);
1524                 if (ret < 0)
1525                         return ret;
1526                 if (ret == 0)
1527                         return -EINVAL;
1528                 sensor->prev_sysclk = ret;
1529         }
1530
1531         if (!cap_bandfilt)
1532                 return -EINVAL;
1533
1534         cap_maxband = (int)((cap_vts - 4) / cap_bandfilt);
1535
1536         /* calculate capture shutter/gain16 */
1537         if (average > sensor->ae_low && average < sensor->ae_high) {
1538                 /* in stable range */
1539                 cap_gain16_shutter =
1540                         prev_gain16 * prev_shutter *
1541                         cap_sysclk / sensor->prev_sysclk *
1542                         sensor->prev_hts / cap_hts *
1543                         sensor->ae_target / average;
1544         } else {
1545                 cap_gain16_shutter =
1546                         prev_gain16 * prev_shutter *
1547                         cap_sysclk / sensor->prev_sysclk *
1548                         sensor->prev_hts / cap_hts;
1549         }
1550
1551         /* gain to shutter */
1552         if (cap_gain16_shutter < (cap_bandfilt * 16)) {
1553                 /* shutter < 1/100 */
1554                 cap_shutter = cap_gain16_shutter / 16;
1555                 if (cap_shutter < 1)
1556                         cap_shutter = 1;
1557
1558                 cap_gain16 = cap_gain16_shutter / cap_shutter;
1559                 if (cap_gain16 < 16)
1560                         cap_gain16 = 16;
1561         } else {
1562                 if (cap_gain16_shutter > (cap_bandfilt * cap_maxband * 16)) {
1563                         /* exposure reach max */
1564                         cap_shutter = cap_bandfilt * cap_maxband;
1565                         if (!cap_shutter)
1566                                 return -EINVAL;
1567
1568                         cap_gain16 = cap_gain16_shutter / cap_shutter;
1569                 } else {
1570                         /* 1/100 < (cap_shutter = n/100) =< max */
1571                         cap_shutter =
1572                                 ((int)(cap_gain16_shutter / 16 / cap_bandfilt))
1573                                 * cap_bandfilt;
1574                         if (!cap_shutter)
1575                                 return -EINVAL;
1576
1577                         cap_gain16 = cap_gain16_shutter / cap_shutter;
1578                 }
1579         }
1580
1581         /* set capture gain */
1582         ret = __v4l2_ctrl_s_ctrl(sensor->ctrls.gain, cap_gain16);
1583         if (ret)
1584                 return ret;
1585
1586         /* write capture shutter */
1587         if (cap_shutter > (cap_vts - 4)) {
1588                 cap_vts = cap_shutter + 4;
1589                 ret = ov5640_set_vts(sensor, cap_vts);
1590                 if (ret < 0)
1591                         return ret;
1592         }
1593
1594         /* set exposure */
1595         return __v4l2_ctrl_s_ctrl(sensor->ctrls.exposure, cap_shutter);
1596 }
1597
1598 /*
1599  * if sensor changes inside scaling or subsampling
1600  * change mode directly
1601  */
1602 static int ov5640_set_mode_direct(struct ov5640_dev *sensor,
1603                                   const struct ov5640_mode_info *mode,
1604                                   s32 exposure)
1605 {
1606         int ret;
1607
1608         if (!mode->reg_data)
1609                 return -EINVAL;
1610
1611         /* Write capture setting */
1612         ret = ov5640_load_regs(sensor, mode);
1613         if (ret < 0)
1614                 return ret;
1615
1616         /* turn auto gain/exposure back on for direct mode */
1617         ret = __v4l2_ctrl_s_ctrl(sensor->ctrls.auto_gain, 1);
1618         if (ret)
1619                 return ret;
1620
1621         return __v4l2_ctrl_s_ctrl(sensor->ctrls.auto_exp, exposure);
1622 }
1623
1624 static int ov5640_set_mode(struct ov5640_dev *sensor,
1625                            const struct ov5640_mode_info *orig_mode)
1626 {
1627         const struct ov5640_mode_info *mode = sensor->current_mode;
1628         enum ov5640_downsize_mode dn_mode, orig_dn_mode;
1629         s32 exposure;
1630         int ret;
1631
1632         dn_mode = mode->dn_mode;
1633         orig_dn_mode = orig_mode->dn_mode;
1634
1635         /* auto gain and exposure must be turned off when changing modes */
1636         ret = __v4l2_ctrl_s_ctrl(sensor->ctrls.auto_gain, 0);
1637         if (ret)
1638                 return ret;
1639
1640         exposure = sensor->ctrls.auto_exp->val;
1641         ret = ov5640_set_exposure(sensor, V4L2_EXPOSURE_MANUAL);
1642         if (ret)
1643                 return ret;
1644
1645         if ((dn_mode == SUBSAMPLING && orig_dn_mode == SCALING) ||
1646             (dn_mode == SCALING && orig_dn_mode == SUBSAMPLING)) {
1647                 /*
1648                  * change between subsampling and scaling
1649                  * go through exposure calucation
1650                  */
1651                 ret = ov5640_set_mode_exposure_calc(sensor, mode);
1652         } else {
1653                 /*
1654                  * change inside subsampling or scaling
1655                  * download firmware directly
1656                  */
1657                 ret = ov5640_set_mode_direct(sensor, mode, exposure);
1658         }
1659
1660         if (ret < 0)
1661                 return ret;
1662
1663         ret = ov5640_set_timings(sensor, mode);
1664         if (ret < 0)
1665                 return ret;
1666
1667         ret = ov5640_set_binning(sensor, dn_mode != SCALING);
1668         if (ret < 0)
1669                 return ret;
1670         ret = ov5640_set_ae_target(sensor, sensor->ae_target);
1671         if (ret < 0)
1672                 return ret;
1673         ret = ov5640_get_light_freq(sensor);
1674         if (ret < 0)
1675                 return ret;
1676         ret = ov5640_set_bandingfilter(sensor);
1677         if (ret < 0)
1678                 return ret;
1679         ret = ov5640_set_virtual_channel(sensor);
1680         if (ret < 0)
1681                 return ret;
1682
1683         sensor->pending_mode_change = false;
1684
1685         return 0;
1686 }
1687
1688 static int ov5640_set_framefmt(struct ov5640_dev *sensor,
1689                                struct v4l2_mbus_framefmt *format);
1690
1691 /* restore the last set video mode after chip power-on */
1692 static int ov5640_restore_mode(struct ov5640_dev *sensor)
1693 {
1694         int ret;
1695
1696         /* first load the initial register values */
1697         ret = ov5640_load_regs(sensor, &ov5640_mode_init_data);
1698         if (ret < 0)
1699                 return ret;
1700
1701         ret = ov5640_mod_reg(sensor, OV5640_REG_SYS_ROOT_DIVIDER, 0x3f,
1702                              (ilog2(OV5640_SCLK2X_ROOT_DIVIDER_DEFAULT) << 2) |
1703                              ilog2(OV5640_SCLK_ROOT_DIVIDER_DEFAULT));
1704         if (ret)
1705                 return ret;
1706
1707         /* now restore the last capture mode */
1708         ret = ov5640_set_mode(sensor, &ov5640_mode_init_data);
1709         if (ret < 0)
1710                 return ret;
1711
1712         return ov5640_set_framefmt(sensor, &sensor->fmt);
1713 }
1714
1715 static void ov5640_power(struct ov5640_dev *sensor, bool enable)
1716 {
1717         gpiod_set_value_cansleep(sensor->pwdn_gpio, enable ? 0 : 1);
1718 }
1719
1720 static void ov5640_reset(struct ov5640_dev *sensor)
1721 {
1722         if (!sensor->reset_gpio)
1723                 return;
1724
1725         gpiod_set_value_cansleep(sensor->reset_gpio, 0);
1726
1727         /* camera power cycle */
1728         ov5640_power(sensor, false);
1729         usleep_range(5000, 10000);
1730         ov5640_power(sensor, true);
1731         usleep_range(5000, 10000);
1732
1733         gpiod_set_value_cansleep(sensor->reset_gpio, 1);
1734         usleep_range(1000, 2000);
1735
1736         gpiod_set_value_cansleep(sensor->reset_gpio, 0);
1737         usleep_range(5000, 10000);
1738 }
1739
1740 static int ov5640_set_power_on(struct ov5640_dev *sensor)
1741 {
1742         struct i2c_client *client = sensor->i2c_client;
1743         int ret;
1744
1745         ret = clk_prepare_enable(sensor->xclk);
1746         if (ret) {
1747                 dev_err(&client->dev, "%s: failed to enable clock\n",
1748                         __func__);
1749                 return ret;
1750         }
1751
1752         ret = regulator_bulk_enable(OV5640_NUM_SUPPLIES,
1753                                     sensor->supplies);
1754         if (ret) {
1755                 dev_err(&client->dev, "%s: failed to enable regulators\n",
1756                         __func__);
1757                 goto xclk_off;
1758         }
1759
1760         ov5640_reset(sensor);
1761         ov5640_power(sensor, true);
1762
1763         ret = ov5640_init_slave_id(sensor);
1764         if (ret)
1765                 goto power_off;
1766
1767         return 0;
1768
1769 power_off:
1770         ov5640_power(sensor, false);
1771         regulator_bulk_disable(OV5640_NUM_SUPPLIES, sensor->supplies);
1772 xclk_off:
1773         clk_disable_unprepare(sensor->xclk);
1774         return ret;
1775 }
1776
1777 static void ov5640_set_power_off(struct ov5640_dev *sensor)
1778 {
1779         ov5640_power(sensor, false);
1780         regulator_bulk_disable(OV5640_NUM_SUPPLIES, sensor->supplies);
1781         clk_disable_unprepare(sensor->xclk);
1782 }
1783
1784 static int ov5640_set_power(struct ov5640_dev *sensor, bool on)
1785 {
1786         int ret = 0;
1787
1788         if (on) {
1789                 ret = ov5640_set_power_on(sensor);
1790                 if (ret)
1791                         return ret;
1792
1793                 ret = ov5640_restore_mode(sensor);
1794                 if (ret)
1795                         goto power_off;
1796
1797                 if (sensor->ep.bus_type == V4L2_MBUS_CSI2) {
1798                         /*
1799                          * start streaming briefly followed by stream off in
1800                          * order to coax the clock lane into LP-11 state.
1801                          */
1802                         ret = ov5640_set_stream_mipi(sensor, true);
1803                         if (ret)
1804                                 goto power_off;
1805                         usleep_range(1000, 2000);
1806                         ret = ov5640_set_stream_mipi(sensor, false);
1807                         if (ret)
1808                                 goto power_off;
1809                 }
1810
1811                 return 0;
1812         }
1813
1814 power_off:
1815         ov5640_set_power_off(sensor);
1816         return ret;
1817 }
1818
1819 /* --------------- Subdev Operations --------------- */
1820
1821 static int ov5640_s_power(struct v4l2_subdev *sd, int on)
1822 {
1823         struct ov5640_dev *sensor = to_ov5640_dev(sd);
1824         int ret = 0;
1825
1826         mutex_lock(&sensor->lock);
1827
1828         /*
1829          * If the power count is modified from 0 to != 0 or from != 0 to 0,
1830          * update the power state.
1831          */
1832         if (sensor->power_count == !on) {
1833                 ret = ov5640_set_power(sensor, !!on);
1834                 if (ret)
1835                         goto out;
1836         }
1837
1838         /* Update the power count. */
1839         sensor->power_count += on ? 1 : -1;
1840         WARN_ON(sensor->power_count < 0);
1841 out:
1842         mutex_unlock(&sensor->lock);
1843
1844         if (on && !ret && sensor->power_count == 1) {
1845                 /* restore controls */
1846                 ret = v4l2_ctrl_handler_setup(&sensor->ctrls.handler);
1847         }
1848
1849         return ret;
1850 }
1851
1852 static int ov5640_try_frame_interval(struct ov5640_dev *sensor,
1853                                      struct v4l2_fract *fi,
1854                                      u32 width, u32 height)
1855 {
1856         const struct ov5640_mode_info *mode;
1857         u32 minfps, maxfps, fps;
1858         int ret;
1859
1860         minfps = ov5640_framerates[OV5640_15_FPS];
1861         maxfps = ov5640_framerates[OV5640_30_FPS];
1862
1863         if (fi->numerator == 0) {
1864                 fi->denominator = maxfps;
1865                 fi->numerator = 1;
1866                 return OV5640_30_FPS;
1867         }
1868
1869         fps = DIV_ROUND_CLOSEST(fi->denominator, fi->numerator);
1870
1871         fi->numerator = 1;
1872         if (fps > maxfps)
1873                 fi->denominator = maxfps;
1874         else if (fps < minfps)
1875                 fi->denominator = minfps;
1876         else if (2 * fps >= 2 * minfps + (maxfps - minfps))
1877                 fi->denominator = maxfps;
1878         else
1879                 fi->denominator = minfps;
1880
1881         ret = (fi->denominator == minfps) ? OV5640_15_FPS : OV5640_30_FPS;
1882
1883         mode = ov5640_find_mode(sensor, ret, width, height, false);
1884         return mode ? ret : -EINVAL;
1885 }
1886
1887 static int ov5640_get_fmt(struct v4l2_subdev *sd,
1888                           struct v4l2_subdev_pad_config *cfg,
1889                           struct v4l2_subdev_format *format)
1890 {
1891         struct ov5640_dev *sensor = to_ov5640_dev(sd);
1892         struct v4l2_mbus_framefmt *fmt;
1893
1894         if (format->pad != 0)
1895                 return -EINVAL;
1896
1897         mutex_lock(&sensor->lock);
1898
1899         if (format->which == V4L2_SUBDEV_FORMAT_TRY)
1900                 fmt = v4l2_subdev_get_try_format(&sensor->sd, cfg,
1901                                                  format->pad);
1902         else
1903                 fmt = &sensor->fmt;
1904
1905         format->format = *fmt;
1906
1907         mutex_unlock(&sensor->lock);
1908
1909         return 0;
1910 }
1911
1912 static int ov5640_try_fmt_internal(struct v4l2_subdev *sd,
1913                                    struct v4l2_mbus_framefmt *fmt,
1914                                    enum ov5640_frame_rate fr,
1915                                    const struct ov5640_mode_info **new_mode)
1916 {
1917         struct ov5640_dev *sensor = to_ov5640_dev(sd);
1918         const struct ov5640_mode_info *mode;
1919         int i;
1920
1921         mode = ov5640_find_mode(sensor, fr, fmt->width, fmt->height, true);
1922         if (!mode)
1923                 return -EINVAL;
1924         fmt->width = mode->hact;
1925         fmt->height = mode->vact;
1926
1927         if (new_mode)
1928                 *new_mode = mode;
1929
1930         for (i = 0; i < ARRAY_SIZE(ov5640_formats); i++)
1931                 if (ov5640_formats[i].code == fmt->code)
1932                         break;
1933         if (i >= ARRAY_SIZE(ov5640_formats))
1934                 i = 0;
1935
1936         fmt->code = ov5640_formats[i].code;
1937         fmt->colorspace = ov5640_formats[i].colorspace;
1938         fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
1939         fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
1940         fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
1941
1942         return 0;
1943 }
1944
1945 static int ov5640_set_fmt(struct v4l2_subdev *sd,
1946                           struct v4l2_subdev_pad_config *cfg,
1947                           struct v4l2_subdev_format *format)
1948 {
1949         struct ov5640_dev *sensor = to_ov5640_dev(sd);
1950         const struct ov5640_mode_info *new_mode;
1951         struct v4l2_mbus_framefmt *mbus_fmt = &format->format;
1952         int ret;
1953
1954         if (format->pad != 0)
1955                 return -EINVAL;
1956
1957         mutex_lock(&sensor->lock);
1958
1959         if (sensor->streaming) {
1960                 ret = -EBUSY;
1961                 goto out;
1962         }
1963
1964         ret = ov5640_try_fmt_internal(sd, mbus_fmt,
1965                                       sensor->current_fr, &new_mode);
1966         if (ret)
1967                 goto out;
1968
1969         if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
1970                 struct v4l2_mbus_framefmt *fmt =
1971                         v4l2_subdev_get_try_format(sd, cfg, 0);
1972
1973                 *fmt = *mbus_fmt;
1974                 goto out;
1975         }
1976
1977         sensor->current_mode = new_mode;
1978         sensor->fmt = *mbus_fmt;
1979         sensor->pending_mode_change = true;
1980 out:
1981         mutex_unlock(&sensor->lock);
1982         return ret;
1983 }
1984
1985 static int ov5640_set_framefmt(struct ov5640_dev *sensor,
1986                                struct v4l2_mbus_framefmt *format)
1987 {
1988         int ret = 0;
1989         bool is_rgb = false;
1990         bool is_jpeg = false;
1991         u8 val;
1992
1993         switch (format->code) {
1994         case MEDIA_BUS_FMT_UYVY8_2X8:
1995                 /* YUV422, UYVY */
1996                 val = 0x3f;
1997                 break;
1998         case MEDIA_BUS_FMT_YUYV8_2X8:
1999                 /* YUV422, YUYV */
2000                 val = 0x30;
2001                 break;
2002         case MEDIA_BUS_FMT_RGB565_2X8_LE:
2003                 /* RGB565 {g[2:0],b[4:0]},{r[4:0],g[5:3]} */
2004                 val = 0x6F;
2005                 is_rgb = true;
2006                 break;
2007         case MEDIA_BUS_FMT_RGB565_2X8_BE:
2008                 /* RGB565 {r[4:0],g[5:3]},{g[2:0],b[4:0]} */
2009                 val = 0x61;
2010                 is_rgb = true;
2011                 break;
2012         case MEDIA_BUS_FMT_JPEG_1X8:
2013                 /* YUV422, YUYV */
2014                 val = 0x30;
2015                 is_jpeg = true;
2016                 break;
2017         default:
2018                 return -EINVAL;
2019         }
2020
2021         /* FORMAT CONTROL00: YUV and RGB formatting */
2022         ret = ov5640_write_reg(sensor, OV5640_REG_FORMAT_CONTROL00, val);
2023         if (ret)
2024                 return ret;
2025
2026         /* FORMAT MUX CONTROL: ISP YUV or RGB */
2027         ret = ov5640_write_reg(sensor, OV5640_REG_ISP_FORMAT_MUX_CTRL,
2028                                is_rgb ? 0x01 : 0x00);
2029         if (ret)
2030                 return ret;
2031
2032         /*
2033          * TIMING TC REG21:
2034          * - [5]:       JPEG enable
2035          */
2036         ret = ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG21,
2037                              BIT(5), is_jpeg ? BIT(5) : 0);
2038         if (ret)
2039                 return ret;
2040
2041         /*
2042          * SYSTEM RESET02:
2043          * - [4]:       Reset JFIFO
2044          * - [3]:       Reset SFIFO
2045          * - [2]:       Reset JPEG
2046          */
2047         ret = ov5640_mod_reg(sensor, OV5640_REG_SYS_RESET02,
2048                              BIT(4) | BIT(3) | BIT(2),
2049                              is_jpeg ? 0 : (BIT(4) | BIT(3) | BIT(2)));
2050         if (ret)
2051                 return ret;
2052
2053         /*
2054          * CLOCK ENABLE02:
2055          * - [5]:       Enable JPEG 2x clock
2056          * - [3]:       Enable JPEG clock
2057          */
2058         return ov5640_mod_reg(sensor, OV5640_REG_SYS_CLOCK_ENABLE02,
2059                               BIT(5) | BIT(3),
2060                               is_jpeg ? (BIT(5) | BIT(3)) : 0);
2061 }
2062
2063 /*
2064  * Sensor Controls.
2065  */
2066
2067 static int ov5640_set_ctrl_hue(struct ov5640_dev *sensor, int value)
2068 {
2069         int ret;
2070
2071         if (value) {
2072                 ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0,
2073                                      BIT(0), BIT(0));
2074                 if (ret)
2075                         return ret;
2076                 ret = ov5640_write_reg16(sensor, OV5640_REG_SDE_CTRL1, value);
2077         } else {
2078                 ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0, BIT(0), 0);
2079         }
2080
2081         return ret;
2082 }
2083
2084 static int ov5640_set_ctrl_contrast(struct ov5640_dev *sensor, int value)
2085 {
2086         int ret;
2087
2088         if (value) {
2089                 ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0,
2090                                      BIT(2), BIT(2));
2091                 if (ret)
2092                         return ret;
2093                 ret = ov5640_write_reg(sensor, OV5640_REG_SDE_CTRL5,
2094                                        value & 0xff);
2095         } else {
2096                 ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0, BIT(2), 0);
2097         }
2098
2099         return ret;
2100 }
2101
2102 static int ov5640_set_ctrl_saturation(struct ov5640_dev *sensor, int value)
2103 {
2104         int ret;
2105
2106         if (value) {
2107                 ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0,
2108                                      BIT(1), BIT(1));
2109                 if (ret)
2110                         return ret;
2111                 ret = ov5640_write_reg(sensor, OV5640_REG_SDE_CTRL3,
2112                                        value & 0xff);
2113                 if (ret)
2114                         return ret;
2115                 ret = ov5640_write_reg(sensor, OV5640_REG_SDE_CTRL4,
2116                                        value & 0xff);
2117         } else {
2118                 ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0, BIT(1), 0);
2119         }
2120
2121         return ret;
2122 }
2123
2124 static int ov5640_set_ctrl_white_balance(struct ov5640_dev *sensor, int awb)
2125 {
2126         int ret;
2127
2128         ret = ov5640_mod_reg(sensor, OV5640_REG_AWB_MANUAL_CTRL,
2129                              BIT(0), awb ? 0 : 1);
2130         if (ret)
2131                 return ret;
2132
2133         if (!awb) {
2134                 u16 red = (u16)sensor->ctrls.red_balance->val;
2135                 u16 blue = (u16)sensor->ctrls.blue_balance->val;
2136
2137                 ret = ov5640_write_reg16(sensor, OV5640_REG_AWB_R_GAIN, red);
2138                 if (ret)
2139                         return ret;
2140                 ret = ov5640_write_reg16(sensor, OV5640_REG_AWB_B_GAIN, blue);
2141         }
2142
2143         return ret;
2144 }
2145
2146 static int ov5640_set_ctrl_exposure(struct ov5640_dev *sensor, int exp)
2147 {
2148         struct ov5640_ctrls *ctrls = &sensor->ctrls;
2149         bool auto_exposure = (exp == V4L2_EXPOSURE_AUTO);
2150         int ret = 0;
2151
2152         if (ctrls->auto_exp->is_new) {
2153                 ret = ov5640_mod_reg(sensor, OV5640_REG_AEC_PK_MANUAL,
2154                                      BIT(0), auto_exposure ? 0 : BIT(0));
2155                 if (ret)
2156                         return ret;
2157         }
2158
2159         if (!auto_exposure && ctrls->exposure->is_new) {
2160                 u16 max_exp;
2161
2162                 ret = ov5640_read_reg16(sensor, OV5640_REG_AEC_PK_VTS,
2163                                         &max_exp);
2164                 if (ret)
2165                         return ret;
2166                 ret = ov5640_get_vts(sensor);
2167                 if (ret < 0)
2168                         return ret;
2169                 max_exp += ret;
2170                 ret = 0;
2171
2172                 if (ctrls->exposure->val < max_exp)
2173                         ret = ov5640_set_exposure(sensor, ctrls->exposure->val);
2174         }
2175
2176         return ret;
2177 }
2178
2179 static int ov5640_set_ctrl_gain(struct ov5640_dev *sensor, int auto_gain)
2180 {
2181         struct ov5640_ctrls *ctrls = &sensor->ctrls;
2182         int ret = 0;
2183
2184         if (ctrls->auto_gain->is_new) {
2185                 ret = ov5640_mod_reg(sensor, OV5640_REG_AEC_PK_MANUAL,
2186                                      BIT(1),
2187                                      ctrls->auto_gain->val ? 0 : BIT(1));
2188                 if (ret)
2189                         return ret;
2190         }
2191
2192         if (!auto_gain && ctrls->gain->is_new) {
2193                 u16 gain = (u16)ctrls->gain->val;
2194
2195                 ret = ov5640_write_reg16(sensor, OV5640_REG_AEC_PK_REAL_GAIN,
2196                                          gain & 0x3ff);
2197         }
2198
2199         return ret;
2200 }
2201
2202 static int ov5640_set_ctrl_test_pattern(struct ov5640_dev *sensor, int value)
2203 {
2204         return ov5640_mod_reg(sensor, OV5640_REG_PRE_ISP_TEST_SET1,
2205                               0xa4, value ? 0xa4 : 0);
2206 }
2207
2208 static int ov5640_set_ctrl_light_freq(struct ov5640_dev *sensor, int value)
2209 {
2210         int ret;
2211
2212         ret = ov5640_mod_reg(sensor, OV5640_REG_HZ5060_CTRL01, BIT(7),
2213                              (value == V4L2_CID_POWER_LINE_FREQUENCY_AUTO) ?
2214                              0 : BIT(7));
2215         if (ret)
2216                 return ret;
2217
2218         return ov5640_mod_reg(sensor, OV5640_REG_HZ5060_CTRL00, BIT(2),
2219                               (value == V4L2_CID_POWER_LINE_FREQUENCY_50HZ) ?
2220                               BIT(2) : 0);
2221 }
2222
2223 static int ov5640_set_ctrl_hflip(struct ov5640_dev *sensor, int value)
2224 {
2225         /*
2226          * If sensor is mounted upside down, mirror logic is inversed.
2227          *
2228          * Sensor is a BSI (Back Side Illuminated) one,
2229          * so image captured is physically mirrored.
2230          * This is why mirror logic is inversed in
2231          * order to cancel this mirror effect.
2232          */
2233
2234         /*
2235          * TIMING TC REG21:
2236          * - [2]:       ISP mirror
2237          * - [1]:       Sensor mirror
2238          */
2239         return ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG21,
2240                               BIT(2) | BIT(1),
2241                               (!(value ^ sensor->upside_down)) ?
2242                               (BIT(2) | BIT(1)) : 0);
2243 }
2244
2245 static int ov5640_set_ctrl_vflip(struct ov5640_dev *sensor, int value)
2246 {
2247         /* If sensor is mounted upside down, flip logic is inversed */
2248
2249         /*
2250          * TIMING TC REG20:
2251          * - [2]:       ISP vflip
2252          * - [1]:       Sensor vflip
2253          */
2254         return ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG20,
2255                               BIT(2) | BIT(1),
2256                               (value ^ sensor->upside_down) ?
2257                               (BIT(2) | BIT(1)) : 0);
2258 }
2259
2260 static int ov5640_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
2261 {
2262         struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
2263         struct ov5640_dev *sensor = to_ov5640_dev(sd);
2264         int val;
2265
2266         /* v4l2_ctrl_lock() locks our own mutex */
2267
2268         switch (ctrl->id) {
2269         case V4L2_CID_AUTOGAIN:
2270                 if (!ctrl->val)
2271                         return 0;
2272                 val = ov5640_get_gain(sensor);
2273                 if (val < 0)
2274                         return val;
2275                 sensor->ctrls.gain->val = val;
2276                 break;
2277         case V4L2_CID_EXPOSURE_AUTO:
2278                 if (ctrl->val == V4L2_EXPOSURE_MANUAL)
2279                         return 0;
2280                 val = ov5640_get_exposure(sensor);
2281                 if (val < 0)
2282                         return val;
2283                 sensor->ctrls.exposure->val = val;
2284                 break;
2285         }
2286
2287         return 0;
2288 }
2289
2290 static int ov5640_s_ctrl(struct v4l2_ctrl *ctrl)
2291 {
2292         struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
2293         struct ov5640_dev *sensor = to_ov5640_dev(sd);
2294         int ret;
2295
2296         /* v4l2_ctrl_lock() locks our own mutex */
2297
2298         /*
2299          * If the device is not powered up by the host driver do
2300          * not apply any controls to H/W at this time. Instead
2301          * the controls will be restored right after power-up.
2302          */
2303         if (sensor->power_count == 0)
2304                 return 0;
2305
2306         switch (ctrl->id) {
2307         case V4L2_CID_AUTOGAIN:
2308                 ret = ov5640_set_ctrl_gain(sensor, ctrl->val);
2309                 break;
2310         case V4L2_CID_EXPOSURE_AUTO:
2311                 ret = ov5640_set_ctrl_exposure(sensor, ctrl->val);
2312                 break;
2313         case V4L2_CID_AUTO_WHITE_BALANCE:
2314                 ret = ov5640_set_ctrl_white_balance(sensor, ctrl->val);
2315                 break;
2316         case V4L2_CID_HUE:
2317                 ret = ov5640_set_ctrl_hue(sensor, ctrl->val);
2318                 break;
2319         case V4L2_CID_CONTRAST:
2320                 ret = ov5640_set_ctrl_contrast(sensor, ctrl->val);
2321                 break;
2322         case V4L2_CID_SATURATION:
2323                 ret = ov5640_set_ctrl_saturation(sensor, ctrl->val);
2324                 break;
2325         case V4L2_CID_TEST_PATTERN:
2326                 ret = ov5640_set_ctrl_test_pattern(sensor, ctrl->val);
2327                 break;
2328         case V4L2_CID_POWER_LINE_FREQUENCY:
2329                 ret = ov5640_set_ctrl_light_freq(sensor, ctrl->val);
2330                 break;
2331         case V4L2_CID_HFLIP:
2332                 ret = ov5640_set_ctrl_hflip(sensor, ctrl->val);
2333                 break;
2334         case V4L2_CID_VFLIP:
2335                 ret = ov5640_set_ctrl_vflip(sensor, ctrl->val);
2336                 break;
2337         default:
2338                 ret = -EINVAL;
2339                 break;
2340         }
2341
2342         return ret;
2343 }
2344
2345 static const struct v4l2_ctrl_ops ov5640_ctrl_ops = {
2346         .g_volatile_ctrl = ov5640_g_volatile_ctrl,
2347         .s_ctrl = ov5640_s_ctrl,
2348 };
2349
2350 static const char * const test_pattern_menu[] = {
2351         "Disabled",
2352         "Color bars",
2353 };
2354
2355 static int ov5640_init_controls(struct ov5640_dev *sensor)
2356 {
2357         const struct v4l2_ctrl_ops *ops = &ov5640_ctrl_ops;
2358         struct ov5640_ctrls *ctrls = &sensor->ctrls;
2359         struct v4l2_ctrl_handler *hdl = &ctrls->handler;
2360         int ret;
2361
2362         v4l2_ctrl_handler_init(hdl, 32);
2363
2364         /* we can use our own mutex for the ctrl lock */
2365         hdl->lock = &sensor->lock;
2366
2367         /* Auto/manual white balance */
2368         ctrls->auto_wb = v4l2_ctrl_new_std(hdl, ops,
2369                                            V4L2_CID_AUTO_WHITE_BALANCE,
2370                                            0, 1, 1, 1);
2371         ctrls->blue_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BLUE_BALANCE,
2372                                                 0, 4095, 1, 0);
2373         ctrls->red_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_RED_BALANCE,
2374                                                0, 4095, 1, 0);
2375         /* Auto/manual exposure */
2376         ctrls->auto_exp = v4l2_ctrl_new_std_menu(hdl, ops,
2377                                                  V4L2_CID_EXPOSURE_AUTO,
2378                                                  V4L2_EXPOSURE_MANUAL, 0,
2379                                                  V4L2_EXPOSURE_AUTO);
2380         ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
2381                                             0, 65535, 1, 0);
2382         /* Auto/manual gain */
2383         ctrls->auto_gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTOGAIN,
2384                                              0, 1, 1, 1);
2385         ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN,
2386                                         0, 1023, 1, 0);
2387
2388         ctrls->saturation = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SATURATION,
2389                                               0, 255, 1, 64);
2390         ctrls->hue = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HUE,
2391                                        0, 359, 1, 0);
2392         ctrls->contrast = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_CONTRAST,
2393                                             0, 255, 1, 0);
2394         ctrls->test_pattern =
2395                 v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
2396                                              ARRAY_SIZE(test_pattern_menu) - 1,
2397                                              0, 0, test_pattern_menu);
2398         ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP,
2399                                          0, 1, 1, 0);
2400         ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP,
2401                                          0, 1, 1, 0);
2402
2403         ctrls->light_freq =
2404                 v4l2_ctrl_new_std_menu(hdl, ops,
2405                                        V4L2_CID_POWER_LINE_FREQUENCY,
2406                                        V4L2_CID_POWER_LINE_FREQUENCY_AUTO, 0,
2407                                        V4L2_CID_POWER_LINE_FREQUENCY_50HZ);
2408
2409         if (hdl->error) {
2410                 ret = hdl->error;
2411                 goto free_ctrls;
2412         }
2413
2414         ctrls->gain->flags |= V4L2_CTRL_FLAG_VOLATILE;
2415         ctrls->exposure->flags |= V4L2_CTRL_FLAG_VOLATILE;
2416
2417         v4l2_ctrl_auto_cluster(3, &ctrls->auto_wb, 0, false);
2418         v4l2_ctrl_auto_cluster(2, &ctrls->auto_gain, 0, true);
2419         v4l2_ctrl_auto_cluster(2, &ctrls->auto_exp, 1, true);
2420
2421         sensor->sd.ctrl_handler = hdl;
2422         return 0;
2423
2424 free_ctrls:
2425         v4l2_ctrl_handler_free(hdl);
2426         return ret;
2427 }
2428
2429 static int ov5640_enum_frame_size(struct v4l2_subdev *sd,
2430                                   struct v4l2_subdev_pad_config *cfg,
2431                                   struct v4l2_subdev_frame_size_enum *fse)
2432 {
2433         if (fse->pad != 0)
2434                 return -EINVAL;
2435         if (fse->index >= OV5640_NUM_MODES)
2436                 return -EINVAL;
2437
2438         fse->min_width =
2439                 ov5640_mode_data[0][fse->index].hact;
2440         fse->max_width = fse->min_width;
2441         fse->min_height =
2442                 ov5640_mode_data[0][fse->index].vact;
2443         fse->max_height = fse->min_height;
2444
2445         return 0;
2446 }
2447
2448 static int ov5640_enum_frame_interval(
2449         struct v4l2_subdev *sd,
2450         struct v4l2_subdev_pad_config *cfg,
2451         struct v4l2_subdev_frame_interval_enum *fie)
2452 {
2453         struct ov5640_dev *sensor = to_ov5640_dev(sd);
2454         struct v4l2_fract tpf;
2455         int ret;
2456
2457         if (fie->pad != 0)
2458                 return -EINVAL;
2459         if (fie->index >= OV5640_NUM_FRAMERATES)
2460                 return -EINVAL;
2461
2462         tpf.numerator = 1;
2463         tpf.denominator = ov5640_framerates[fie->index];
2464
2465         ret = ov5640_try_frame_interval(sensor, &tpf,
2466                                         fie->width, fie->height);
2467         if (ret < 0)
2468                 return -EINVAL;
2469
2470         fie->interval = tpf;
2471         return 0;
2472 }
2473
2474 static int ov5640_g_frame_interval(struct v4l2_subdev *sd,
2475                                    struct v4l2_subdev_frame_interval *fi)
2476 {
2477         struct ov5640_dev *sensor = to_ov5640_dev(sd);
2478
2479         mutex_lock(&sensor->lock);
2480         fi->interval = sensor->frame_interval;
2481         mutex_unlock(&sensor->lock);
2482
2483         return 0;
2484 }
2485
2486 static int ov5640_s_frame_interval(struct v4l2_subdev *sd,
2487                                    struct v4l2_subdev_frame_interval *fi)
2488 {
2489         struct ov5640_dev *sensor = to_ov5640_dev(sd);
2490         const struct ov5640_mode_info *mode;
2491         int frame_rate, ret = 0;
2492
2493         if (fi->pad != 0)
2494                 return -EINVAL;
2495
2496         mutex_lock(&sensor->lock);
2497
2498         if (sensor->streaming) {
2499                 ret = -EBUSY;
2500                 goto out;
2501         }
2502
2503         mode = sensor->current_mode;
2504
2505         frame_rate = ov5640_try_frame_interval(sensor, &fi->interval,
2506                                                mode->hact, mode->vact);
2507         if (frame_rate < 0)
2508                 frame_rate = OV5640_15_FPS;
2509
2510         sensor->current_fr = frame_rate;
2511         sensor->frame_interval = fi->interval;
2512         sensor->current_mode = ov5640_find_mode(sensor, frame_rate, mode->hact,
2513                                                 mode->vact, true);
2514         sensor->pending_mode_change = true;
2515 out:
2516         mutex_unlock(&sensor->lock);
2517         return ret;
2518 }
2519
2520 static int ov5640_enum_mbus_code(struct v4l2_subdev *sd,
2521                                  struct v4l2_subdev_pad_config *cfg,
2522                                  struct v4l2_subdev_mbus_code_enum *code)
2523 {
2524         if (code->pad != 0)
2525                 return -EINVAL;
2526         if (code->index >= ARRAY_SIZE(ov5640_formats))
2527                 return -EINVAL;
2528
2529         code->code = ov5640_formats[code->index].code;
2530         return 0;
2531 }
2532
2533 static int ov5640_s_stream(struct v4l2_subdev *sd, int enable)
2534 {
2535         struct ov5640_dev *sensor = to_ov5640_dev(sd);
2536         int ret = 0;
2537
2538         mutex_lock(&sensor->lock);
2539
2540         if (sensor->streaming == !enable) {
2541                 if (enable && sensor->pending_mode_change) {
2542                         ret = ov5640_set_mode(sensor, sensor->current_mode);
2543                         if (ret)
2544                                 goto out;
2545
2546                         ret = ov5640_set_framefmt(sensor, &sensor->fmt);
2547                         if (ret)
2548                                 goto out;
2549                 }
2550
2551                 if (sensor->ep.bus_type == V4L2_MBUS_CSI2)
2552                         ret = ov5640_set_stream_mipi(sensor, enable);
2553                 else
2554                         ret = ov5640_set_stream_dvp(sensor, enable);
2555
2556                 if (!ret)
2557                         sensor->streaming = enable;
2558         }
2559 out:
2560         mutex_unlock(&sensor->lock);
2561         return ret;
2562 }
2563
2564 static const struct v4l2_subdev_core_ops ov5640_core_ops = {
2565         .s_power = ov5640_s_power,
2566 };
2567
2568 static const struct v4l2_subdev_video_ops ov5640_video_ops = {
2569         .g_frame_interval = ov5640_g_frame_interval,
2570         .s_frame_interval = ov5640_s_frame_interval,
2571         .s_stream = ov5640_s_stream,
2572 };
2573
2574 static const struct v4l2_subdev_pad_ops ov5640_pad_ops = {
2575         .enum_mbus_code = ov5640_enum_mbus_code,
2576         .get_fmt = ov5640_get_fmt,
2577         .set_fmt = ov5640_set_fmt,
2578         .enum_frame_size = ov5640_enum_frame_size,
2579         .enum_frame_interval = ov5640_enum_frame_interval,
2580 };
2581
2582 static const struct v4l2_subdev_ops ov5640_subdev_ops = {
2583         .core = &ov5640_core_ops,
2584         .video = &ov5640_video_ops,
2585         .pad = &ov5640_pad_ops,
2586 };
2587
2588 static int ov5640_get_regulators(struct ov5640_dev *sensor)
2589 {
2590         int i;
2591
2592         for (i = 0; i < OV5640_NUM_SUPPLIES; i++)
2593                 sensor->supplies[i].supply = ov5640_supply_name[i];
2594
2595         return devm_regulator_bulk_get(&sensor->i2c_client->dev,
2596                                        OV5640_NUM_SUPPLIES,
2597                                        sensor->supplies);
2598 }
2599
2600 static int ov5640_check_chip_id(struct ov5640_dev *sensor)
2601 {
2602         struct i2c_client *client = sensor->i2c_client;
2603         int ret = 0;
2604         u16 chip_id;
2605
2606         ret = ov5640_set_power_on(sensor);
2607         if (ret)
2608                 return ret;
2609
2610         ret = ov5640_read_reg16(sensor, OV5640_REG_CHIP_ID, &chip_id);
2611         if (ret) {
2612                 dev_err(&client->dev, "%s: failed to read chip identifier\n",
2613                         __func__);
2614                 goto power_off;
2615         }
2616
2617         if (chip_id != 0x5640) {
2618                 dev_err(&client->dev, "%s: wrong chip identifier, expected 0x5640, got 0x%x\n",
2619                         __func__, chip_id);
2620                 ret = -ENXIO;
2621         }
2622
2623 power_off:
2624         ov5640_set_power_off(sensor);
2625         return ret;
2626 }
2627
2628 static int ov5640_probe(struct i2c_client *client,
2629                         const struct i2c_device_id *id)
2630 {
2631         struct device *dev = &client->dev;
2632         struct fwnode_handle *endpoint;
2633         struct ov5640_dev *sensor;
2634         struct v4l2_mbus_framefmt *fmt;
2635         u32 rotation;
2636         int ret;
2637
2638         sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
2639         if (!sensor)
2640                 return -ENOMEM;
2641
2642         sensor->i2c_client = client;
2643         fmt = &sensor->fmt;
2644         fmt->code = ov5640_formats[0].code;
2645         fmt->colorspace = ov5640_formats[0].colorspace;
2646         fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
2647         fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
2648         fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
2649         fmt->width = 640;
2650         fmt->height = 480;
2651         fmt->field = V4L2_FIELD_NONE;
2652         sensor->frame_interval.numerator = 1;
2653         sensor->frame_interval.denominator = ov5640_framerates[OV5640_30_FPS];
2654         sensor->current_fr = OV5640_30_FPS;
2655         sensor->current_mode =
2656                 &ov5640_mode_data[OV5640_30_FPS][OV5640_MODE_VGA_640_480];
2657         sensor->pending_mode_change = true;
2658
2659         sensor->ae_target = 52;
2660
2661         /* optional indication of physical rotation of sensor */
2662         ret = fwnode_property_read_u32(dev_fwnode(&client->dev), "rotation",
2663                                        &rotation);
2664         if (!ret) {
2665                 switch (rotation) {
2666                 case 180:
2667                         sensor->upside_down = true;
2668                         /* fall through */
2669                 case 0:
2670                         break;
2671                 default:
2672                         dev_warn(dev, "%u degrees rotation is not supported, ignoring...\n",
2673                                  rotation);
2674                 }
2675         }
2676
2677         endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(&client->dev),
2678                                                   NULL);
2679         if (!endpoint) {
2680                 dev_err(dev, "endpoint node not found\n");
2681                 return -EINVAL;
2682         }
2683
2684         ret = v4l2_fwnode_endpoint_parse(endpoint, &sensor->ep);
2685         fwnode_handle_put(endpoint);
2686         if (ret) {
2687                 dev_err(dev, "Could not parse endpoint\n");
2688                 return ret;
2689         }
2690
2691         /* get system clock (xclk) */
2692         sensor->xclk = devm_clk_get(dev, "xclk");
2693         if (IS_ERR(sensor->xclk)) {
2694                 dev_err(dev, "failed to get xclk\n");
2695                 return PTR_ERR(sensor->xclk);
2696         }
2697
2698         sensor->xclk_freq = clk_get_rate(sensor->xclk);
2699         if (sensor->xclk_freq < OV5640_XCLK_MIN ||
2700             sensor->xclk_freq > OV5640_XCLK_MAX) {
2701                 dev_err(dev, "xclk frequency out of range: %d Hz\n",
2702                         sensor->xclk_freq);
2703                 return -EINVAL;
2704         }
2705
2706         /* request optional power down pin */
2707         sensor->pwdn_gpio = devm_gpiod_get_optional(dev, "powerdown",
2708                                                     GPIOD_OUT_HIGH);
2709         /* request optional reset pin */
2710         sensor->reset_gpio = devm_gpiod_get_optional(dev, "reset",
2711                                                      GPIOD_OUT_HIGH);
2712
2713         v4l2_i2c_subdev_init(&sensor->sd, client, &ov5640_subdev_ops);
2714
2715         sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
2716         sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
2717         sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
2718         ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
2719         if (ret)
2720                 return ret;
2721
2722         ret = ov5640_get_regulators(sensor);
2723         if (ret)
2724                 return ret;
2725
2726         mutex_init(&sensor->lock);
2727
2728         ret = ov5640_check_chip_id(sensor);
2729         if (ret)
2730                 goto entity_cleanup;
2731
2732         ret = ov5640_init_controls(sensor);
2733         if (ret)
2734                 goto entity_cleanup;
2735
2736         ret = v4l2_async_register_subdev(&sensor->sd);
2737         if (ret)
2738                 goto free_ctrls;
2739
2740         return 0;
2741
2742 free_ctrls:
2743         v4l2_ctrl_handler_free(&sensor->ctrls.handler);
2744 entity_cleanup:
2745         mutex_destroy(&sensor->lock);
2746         media_entity_cleanup(&sensor->sd.entity);
2747         return ret;
2748 }
2749
2750 static int ov5640_remove(struct i2c_client *client)
2751 {
2752         struct v4l2_subdev *sd = i2c_get_clientdata(client);
2753         struct ov5640_dev *sensor = to_ov5640_dev(sd);
2754
2755         v4l2_async_unregister_subdev(&sensor->sd);
2756         mutex_destroy(&sensor->lock);
2757         media_entity_cleanup(&sensor->sd.entity);
2758         v4l2_ctrl_handler_free(&sensor->ctrls.handler);
2759
2760         return 0;
2761 }
2762
2763 static const struct i2c_device_id ov5640_id[] = {
2764         {"ov5640", 0},
2765         {},
2766 };
2767 MODULE_DEVICE_TABLE(i2c, ov5640_id);
2768
2769 static const struct of_device_id ov5640_dt_ids[] = {
2770         { .compatible = "ovti,ov5640" },
2771         { /* sentinel */ }
2772 };
2773 MODULE_DEVICE_TABLE(of, ov5640_dt_ids);
2774
2775 static struct i2c_driver ov5640_i2c_driver = {
2776         .driver = {
2777                 .name  = "ov5640",
2778                 .of_match_table = ov5640_dt_ids,
2779         },
2780         .id_table = ov5640_id,
2781         .probe    = ov5640_probe,
2782         .remove   = ov5640_remove,
2783 };
2784
2785 module_i2c_driver(ov5640_i2c_driver);
2786
2787 MODULE_DESCRIPTION("OV5640 MIPI Camera Subdev Driver");
2788 MODULE_LICENSE("GPL");