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