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