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