media/i2c/ov5640.c

0001 // SPDX-License-Identifier: GPL-2.0-or-later
0002 /*
0003  * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved.
0004  * Copyright (C) 2014-2017 Mentor Graphics Inc.
0005  */
0006
0007 #include <linux/clk.h>
0008 #include <linux/clk-provider.h>
0009 #include <linux/clkdev.h>
0010 #include <linux/ctype.h>
0011 #include <linux/delay.h>
0012 #include <linux/device.h>
0013 #include <linux/gpio/consumer.h>
0014 #include <linux/i2c.h>
0015 #include <linux/init.h>
0016 #include <linux/module.h>
0017 #include <linux/of_device.h>
0018 #include <linux/regulator/consumer.h>
0019 #include <linux/slab.h>
0020 #include <linux/types.h>
0021 #include <media/v4l2-async.h>
0022 #include <media/v4l2-ctrls.h>
0023 #include <media/v4l2-device.h>
0024 #include <media/v4l2-event.h>
0025 #include <media/v4l2-fwnode.h>
0026 #include <media/v4l2-subdev.h>
0027
0028 /* min/typical/max system clock (xclk) frequencies */
0029 #define OV5640_XCLK_MIN  6000000
0030 #define OV5640_XCLK_MAX 54000000
0031
0032 #define OV5640_NATIVE_WIDTH     2624
0033 #define OV5640_NATIVE_HEIGHT        1964
0034 #define OV5640_PIXEL_ARRAY_TOP      14
0035 #define OV5640_PIXEL_ARRAY_LEFT     16
0036 #define OV5640_PIXEL_ARRAY_WIDTH    2592
0037 #define OV5640_PIXEL_ARRAY_HEIGHT   1944
0038
0039 /* FIXME: not documented. */
0040 #define OV5640_MIN_VBLANK   24
0041 #define OV5640_MAX_VTS      3375
0042
0043 #define OV5640_DEFAULT_SLAVE_ID 0x3c
0044
0045 #define OV5640_LINK_RATE_MAX        490000000U
0046
0047 #define OV5640_REG_SYS_RESET02      0x3002
0048 #define OV5640_REG_SYS_CLOCK_ENABLE02   0x3006
0049 #define OV5640_REG_SYS_CTRL0        0x3008
0050 #define OV5640_REG_SYS_CTRL0_SW_PWDN    0x42
0051 #define OV5640_REG_SYS_CTRL0_SW_PWUP    0x02
0052 #define OV5640_REG_CHIP_ID      0x300a
0053 #define OV5640_REG_IO_MIPI_CTRL00   0x300e
0054 #define OV5640_REG_PAD_OUTPUT_ENABLE01  0x3017
0055 #define OV5640_REG_PAD_OUTPUT_ENABLE02  0x3018
0056 #define OV5640_REG_PAD_OUTPUT00     0x3019
0057 #define OV5640_REG_SYSTEM_CONTROL1  0x302e
0058 #define OV5640_REG_SC_PLL_CTRL0     0x3034
0059 #define OV5640_REG_SC_PLL_CTRL1     0x3035
0060 #define OV5640_REG_SC_PLL_CTRL2     0x3036
0061 #define OV5640_REG_SC_PLL_CTRL3     0x3037
0062 #define OV5640_REG_SLAVE_ID     0x3100
0063 #define OV5640_REG_SCCB_SYS_CTRL1   0x3103
0064 #define OV5640_REG_SYS_ROOT_DIVIDER 0x3108
0065 #define OV5640_REG_AWB_R_GAIN       0x3400
0066 #define OV5640_REG_AWB_G_GAIN       0x3402
0067 #define OV5640_REG_AWB_B_GAIN       0x3404
0068 #define OV5640_REG_AWB_MANUAL_CTRL  0x3406
0069 #define OV5640_REG_AEC_PK_EXPOSURE_HI   0x3500
0070 #define OV5640_REG_AEC_PK_EXPOSURE_MED  0x3501
0071 #define OV5640_REG_AEC_PK_EXPOSURE_LO   0x3502
0072 #define OV5640_REG_AEC_PK_MANUAL    0x3503
0073 #define OV5640_REG_AEC_PK_REAL_GAIN 0x350a
0074 #define OV5640_REG_AEC_PK_VTS       0x350c
0075 #define OV5640_REG_TIMING_HS        0x3800
0076 #define OV5640_REG_TIMING_VS        0x3802
0077 #define OV5640_REG_TIMING_HW        0x3804
0078 #define OV5640_REG_TIMING_VH        0x3806
0079 #define OV5640_REG_TIMING_DVPHO     0x3808
0080 #define OV5640_REG_TIMING_DVPVO     0x380a
0081 #define OV5640_REG_TIMING_HTS       0x380c
0082 #define OV5640_REG_TIMING_VTS       0x380e
0083 #define OV5640_REG_TIMING_HOFFS     0x3810
0084 #define OV5640_REG_TIMING_VOFFS     0x3812
0085 #define OV5640_REG_TIMING_TC_REG20  0x3820
0086 #define OV5640_REG_TIMING_TC_REG21  0x3821
0087 #define OV5640_REG_AEC_CTRL00       0x3a00
0088 #define OV5640_REG_AEC_B50_STEP     0x3a08
0089 #define OV5640_REG_AEC_B60_STEP     0x3a0a
0090 #define OV5640_REG_AEC_CTRL0D       0x3a0d
0091 #define OV5640_REG_AEC_CTRL0E       0x3a0e
0092 #define OV5640_REG_AEC_CTRL0F       0x3a0f
0093 #define OV5640_REG_AEC_CTRL10       0x3a10
0094 #define OV5640_REG_AEC_CTRL11       0x3a11
0095 #define OV5640_REG_AEC_CTRL1B       0x3a1b
0096 #define OV5640_REG_AEC_CTRL1E       0x3a1e
0097 #define OV5640_REG_AEC_CTRL1F       0x3a1f
0098 #define OV5640_REG_HZ5060_CTRL00    0x3c00
0099 #define OV5640_REG_HZ5060_CTRL01    0x3c01
0100 #define OV5640_REG_SIGMADELTA_CTRL0C    0x3c0c
0101 #define OV5640_REG_FRAME_CTRL01     0x4202
0102 #define OV5640_REG_FORMAT_CONTROL00 0x4300
0103 #define OV5640_REG_VFIFO_HSIZE      0x4602
0104 #define OV5640_REG_VFIFO_VSIZE      0x4604
0105 #define OV5640_REG_JPG_MODE_SELECT  0x4713
0106 #define OV5640_REG_CCIR656_CTRL00   0x4730
0107 #define OV5640_REG_POLARITY_CTRL00  0x4740
0108 #define OV5640_REG_MIPI_CTRL00      0x4800
0109 #define OV5640_REG_DEBUG_MODE       0x4814
0110 #define OV5640_REG_PCLK_PERIOD      0x4837
0111 #define OV5640_REG_ISP_FORMAT_MUX_CTRL  0x501f
0112 #define OV5640_REG_PRE_ISP_TEST_SET1    0x503d
0113 #define OV5640_REG_SDE_CTRL0        0x5580
0114 #define OV5640_REG_SDE_CTRL1        0x5581
0115 #define OV5640_REG_SDE_CTRL3        0x5583
0116 #define OV5640_REG_SDE_CTRL4        0x5584
0117 #define OV5640_REG_SDE_CTRL5        0x5585
0118 #define OV5640_REG_AVG_READOUT      0x56a1
0119
0120 enum ov5640_mode_id {
0121     OV5640_MODE_QQVGA_160_120 = 0,
0122     OV5640_MODE_QCIF_176_144,
0123     OV5640_MODE_QVGA_320_240,
0124     OV5640_MODE_VGA_640_480,
0125     OV5640_MODE_NTSC_720_480,
0126     OV5640_MODE_PAL_720_576,
0127     OV5640_MODE_XGA_1024_768,
0128     OV5640_MODE_720P_1280_720,
0129     OV5640_MODE_1080P_1920_1080,
0130     OV5640_MODE_QSXGA_2592_1944,
0131     OV5640_NUM_MODES,
0132 };
0133
0134 enum ov5640_frame_rate {
0135     OV5640_15_FPS = 0,
0136     OV5640_30_FPS,
0137     OV5640_60_FPS,
0138     OV5640_NUM_FRAMERATES,
0139 };
0140
0141 enum ov5640_pixel_rate_id {
0142     OV5640_PIXEL_RATE_168M,
0143     OV5640_PIXEL_RATE_148M,
0144     OV5640_PIXEL_RATE_124M,
0145     OV5640_PIXEL_RATE_96M,
0146     OV5640_PIXEL_RATE_48M,
0147     OV5640_NUM_PIXEL_RATES,
0148 };
0149
0150 /*
0151  * The chip manual suggests 24/48/96/192 MHz pixel clocks.
0152  *
0153  * 192MHz exceeds the sysclk limits; use 168MHz as maximum pixel rate for
0154  * full resolution mode @15 FPS.
0155  */
0156 static const u32 ov5640_pixel_rates[] = {
0157     [OV5640_PIXEL_RATE_168M] = 168000000,
0158     [OV5640_PIXEL_RATE_148M] = 148000000,
0159     [OV5640_PIXEL_RATE_124M] = 124000000,
0160     [OV5640_PIXEL_RATE_96M] = 96000000,
0161     [OV5640_PIXEL_RATE_48M] = 48000000,
0162 };
0163
0164 /*
0165  * MIPI CSI-2 link frequencies.
0166  *
0167  * Derived from the above defined pixel rate for bpp = (8, 16, 24) and
0168  * data_lanes = (1, 2)
0169  *
0170  * link_freq = (pixel_rate * bpp) / (2 * data_lanes)
0171  */
0172 static const s64 ov5640_csi2_link_freqs[] = {
0173     992000000, 888000000, 768000000, 744000000, 672000000, 672000000,
0174     592000000, 592000000, 576000000, 576000000, 496000000, 496000000,
0175     384000000, 384000000, 384000000, 336000000, 296000000, 288000000,
0176     248000000, 192000000, 192000000, 192000000, 96000000,
0177 };
0178
0179 /* Link freq for default mode: UYVY 16 bpp, 2 data lanes. */
0180 #define OV5640_DEFAULT_LINK_FREQ    13
0181
0182 enum ov5640_format_mux {
0183     OV5640_FMT_MUX_YUV422 = 0,
0184     OV5640_FMT_MUX_RGB,
0185     OV5640_FMT_MUX_DITHER,
0186     OV5640_FMT_MUX_RAW_DPC,
0187     OV5640_FMT_MUX_SNR_RAW,
0188     OV5640_FMT_MUX_RAW_CIP,
0189 };
0190
0191 struct ov5640_pixfmt {
0192     u32 code;
0193     u32 colorspace;
0194     u8 bpp;
0195     u8 ctrl00;
0196     enum ov5640_format_mux mux;
0197 };
0198
0199 static const struct ov5640_pixfmt ov5640_dvp_formats[] = {
0200     {
0201         /* YUV422, YUYV */
0202         .code       = MEDIA_BUS_FMT_JPEG_1X8,
0203         .colorspace = V4L2_COLORSPACE_JPEG,
0204         .bpp        = 16,
0205         .ctrl00     = 0x30,
0206         .mux        = OV5640_FMT_MUX_YUV422,
0207     }, {
0208         /* YUV422, UYVY */
0209         .code       = MEDIA_BUS_FMT_UYVY8_2X8,
0210         .colorspace = V4L2_COLORSPACE_SRGB,
0211         .bpp        = 16,
0212         .ctrl00     = 0x3f,
0213         .mux        = OV5640_FMT_MUX_YUV422,
0214     }, {
0215         /* YUV422, YUYV */
0216         .code       = MEDIA_BUS_FMT_YUYV8_2X8,
0217         .colorspace = V4L2_COLORSPACE_SRGB,
0218         .bpp        = 16,
0219         .ctrl00     = 0x30,
0220         .mux        = OV5640_FMT_MUX_YUV422,
0221     }, {
0222         /* RGB565 {g[2:0],b[4:0]},{r[4:0],g[5:3]} */
0223         .code       = MEDIA_BUS_FMT_RGB565_2X8_LE,
0224         .colorspace = V4L2_COLORSPACE_SRGB,
0225         .bpp        = 16,
0226         .ctrl00     = 0x6f,
0227         .mux        = OV5640_FMT_MUX_RGB,
0228     }, {
0229         /* RGB565 {r[4:0],g[5:3]},{g[2:0],b[4:0]} */
0230         .code       = MEDIA_BUS_FMT_RGB565_2X8_BE,
0231         .colorspace = V4L2_COLORSPACE_SRGB,
0232         .bpp        = 16,
0233         .ctrl00     = 0x61,
0234         .mux        = OV5640_FMT_MUX_RGB,
0235     }, {
0236         /* Raw, BGBG... / GRGR... */
0237         .code       = MEDIA_BUS_FMT_SBGGR8_1X8,
0238         .colorspace = V4L2_COLORSPACE_SRGB,
0239         .bpp        = 8,
0240         .ctrl00     = 0x00,
0241         .mux        = OV5640_FMT_MUX_RAW_DPC,
0242     }, {
0243         /* Raw bayer, GBGB... / RGRG... */
0244         .code       = MEDIA_BUS_FMT_SGBRG8_1X8,
0245         .colorspace = V4L2_COLORSPACE_SRGB,
0246         .bpp        = 8,
0247         .ctrl00     = 0x01,
0248         .mux        = OV5640_FMT_MUX_RAW_DPC,
0249     }, {
0250         /* Raw bayer, GRGR... / BGBG... */
0251         .code       = MEDIA_BUS_FMT_SGRBG8_1X8,
0252         .colorspace = V4L2_COLORSPACE_SRGB,
0253         .bpp        = 8,
0254         .ctrl00     = 0x02,
0255         .mux        = OV5640_FMT_MUX_RAW_DPC,
0256     }, {
0257         /* Raw bayer, RGRG... / GBGB... */
0258         .code       = MEDIA_BUS_FMT_SRGGB8_1X8,
0259         .colorspace = V4L2_COLORSPACE_SRGB,
0260         .bpp        = 8,
0261         .ctrl00     = 0x03,
0262         .mux        = OV5640_FMT_MUX_RAW_DPC,
0263     },
0264     { /* sentinel */ }
0265 };
0266
0267 static const struct ov5640_pixfmt ov5640_csi2_formats[] = {
0268     {
0269         /* YUV422, YUYV */
0270         .code       = MEDIA_BUS_FMT_JPEG_1X8,
0271         .colorspace = V4L2_COLORSPACE_JPEG,
0272         .bpp        = 16,
0273         .ctrl00     = 0x30,
0274         .mux        = OV5640_FMT_MUX_YUV422,
0275     }, {
0276         /* YUV422, UYVY */
0277         .code       = MEDIA_BUS_FMT_UYVY8_1X16,
0278         .colorspace = V4L2_COLORSPACE_SRGB,
0279         .bpp        = 16,
0280         .ctrl00     = 0x3f,
0281         .mux        = OV5640_FMT_MUX_YUV422,
0282     }, {
0283         /* YUV422, YUYV */
0284         .code       = MEDIA_BUS_FMT_YUYV8_1X16,
0285         .colorspace = V4L2_COLORSPACE_SRGB,
0286         .bpp        = 16,
0287         .ctrl00     = 0x30,
0288         .mux        = OV5640_FMT_MUX_YUV422,
0289     }, {
0290         /* RGB565 {g[2:0],b[4:0]},{r[4:0],g[5:3]} */
0291         .code       = MEDIA_BUS_FMT_RGB565_1X16,
0292         .colorspace = V4L2_COLORSPACE_SRGB,
0293         .bpp        = 16,
0294         .ctrl00     = 0x6f,
0295         .mux        = OV5640_FMT_MUX_RGB,
0296     }, {
0297         /* BGR888: RGB */
0298         .code       = MEDIA_BUS_FMT_BGR888_1X24,
0299         .colorspace = V4L2_COLORSPACE_SRGB,
0300         .bpp        = 24,
0301         .ctrl00     = 0x23,
0302         .mux        = OV5640_FMT_MUX_RGB,
0303     }, {
0304         /* Raw, BGBG... / GRGR... */
0305         .code       = MEDIA_BUS_FMT_SBGGR8_1X8,
0306         .colorspace = V4L2_COLORSPACE_SRGB,
0307         .bpp        = 8,
0308         .ctrl00     = 0x00,
0309         .mux        = OV5640_FMT_MUX_RAW_DPC,
0310     }, {
0311         /* Raw bayer, GBGB... / RGRG... */
0312         .code       = MEDIA_BUS_FMT_SGBRG8_1X8,
0313         .colorspace = V4L2_COLORSPACE_SRGB,
0314         .bpp        = 8,
0315         .ctrl00     = 0x01,
0316         .mux        = OV5640_FMT_MUX_RAW_DPC,
0317     }, {
0318         /* Raw bayer, GRGR... / BGBG... */
0319         .code       = MEDIA_BUS_FMT_SGRBG8_1X8,
0320         .colorspace = V4L2_COLORSPACE_SRGB,
0321         .bpp        = 8,
0322         .ctrl00     = 0x02,
0323         .mux        = OV5640_FMT_MUX_RAW_DPC,
0324     }, {
0325         /* Raw bayer, RGRG... / GBGB... */
0326         .code       = MEDIA_BUS_FMT_SRGGB8_1X8,
0327         .colorspace = V4L2_COLORSPACE_SRGB,
0328         .bpp        = 8,
0329         .ctrl00     = 0x03,
0330         .mux        = OV5640_FMT_MUX_RAW_DPC,
0331     },
0332     { /* sentinel */ }
0333 };
0334
0335 /*
0336  * FIXME: remove this when a subdev API becomes available
0337  * to set the MIPI CSI-2 virtual channel.
0338  */
0339 static unsigned int virtual_channel;
0340 module_param(virtual_channel, uint, 0444);
0341 MODULE_PARM_DESC(virtual_channel,
0342          "MIPI CSI-2 virtual channel (0..3), default 0");
0343
0344 static const int ov5640_framerates[] = {
0345     [OV5640_15_FPS] = 15,
0346     [OV5640_30_FPS] = 30,
0347     [OV5640_60_FPS] = 60,
0348 };
0349
0350 /* regulator supplies */
0351 static const char * const ov5640_supply_name[] = {
0352     "DOVDD", /* Digital I/O (1.8V) supply */
0353     "AVDD",  /* Analog (2.8V) supply */
0354     "DVDD",  /* Digital Core (1.5V) supply */
0355 };
0356
0357 #define OV5640_NUM_SUPPLIES ARRAY_SIZE(ov5640_supply_name)
0358
0359 /*
0360  * Image size under 1280 * 960 are SUBSAMPLING
0361  * Image size upper 1280 * 960 are SCALING
0362  */
0363 enum ov5640_downsize_mode {
0364     SUBSAMPLING,
0365     SCALING,
0366 };
0367
0368 struct reg_value {
0369     u16 reg_addr;
0370     u8 val;
0371     u8 mask;
0372     u32 delay_ms;
0373 };
0374
0375 struct ov5640_timings {
0376     /* Analog crop rectangle. */
0377     struct v4l2_rect analog_crop;
0378     /* Visibile crop: from analog crop top-left corner. */
0379     struct v4l2_rect crop;
0380     /* Total pixels per line: width + fixed hblank. */
0381     u32 htot;
0382     /* Default vertical blanking: frame height = height + vblank. */
0383     u32 vblank_def;
0384 };
0385
0386 struct ov5640_mode_info {
0387     enum ov5640_mode_id id;
0388     enum ov5640_downsize_mode dn_mode;
0389     enum ov5640_pixel_rate_id pixel_rate;
0390
0391     unsigned int width;
0392     unsigned int height;
0393
0394     struct ov5640_timings dvp_timings;
0395     struct ov5640_timings csi2_timings;
0396
0397     const struct reg_value *reg_data;
0398     u32 reg_data_size;
0399
0400     /* Used by s_frame_interval only. */
0401     u32 max_fps;
0402     u32 def_fps;
0403 };
0404
0405 struct ov5640_ctrls {
0406     struct v4l2_ctrl_handler handler;
0407     struct v4l2_ctrl *pixel_rate;
0408     struct v4l2_ctrl *link_freq;
0409     struct v4l2_ctrl *hblank;
0410     struct v4l2_ctrl *vblank;
0411     struct {
0412         struct v4l2_ctrl *auto_exp;
0413         struct v4l2_ctrl *exposure;
0414     };
0415     struct {
0416         struct v4l2_ctrl *auto_wb;
0417         struct v4l2_ctrl *blue_balance;
0418         struct v4l2_ctrl *red_balance;
0419     };
0420     struct {
0421         struct v4l2_ctrl *auto_gain;
0422         struct v4l2_ctrl *gain;
0423     };
0424     struct v4l2_ctrl *brightness;
0425     struct v4l2_ctrl *light_freq;
0426     struct v4l2_ctrl *saturation;
0427     struct v4l2_ctrl *contrast;
0428     struct v4l2_ctrl *hue;
0429     struct v4l2_ctrl *test_pattern;
0430     struct v4l2_ctrl *hflip;
0431     struct v4l2_ctrl *vflip;
0432 };
0433
0434 struct ov5640_dev {
0435     struct i2c_client *i2c_client;
0436     struct v4l2_subdev sd;
0437     struct media_pad pad;
0438     struct v4l2_fwnode_endpoint ep; /* the parsed DT endpoint info */
0439     struct clk *xclk; /* system clock to OV5640 */
0440     u32 xclk_freq;
0441
0442     struct regulator_bulk_data supplies[OV5640_NUM_SUPPLIES];
0443     struct gpio_desc *reset_gpio;
0444     struct gpio_desc *pwdn_gpio;
0445     bool   upside_down;
0446
0447     /* lock to protect all members below */
0448     struct mutex lock;
0449
0450     int power_count;
0451
0452     struct v4l2_mbus_framefmt fmt;
0453     bool pending_fmt_change;
0454
0455     const struct ov5640_mode_info *current_mode;
0456     const struct ov5640_mode_info *last_mode;
0457     enum ov5640_frame_rate current_fr;
0458     struct v4l2_fract frame_interval;
0459     s64 current_link_freq;
0460
0461     struct ov5640_ctrls ctrls;
0462
0463     u32 prev_sysclk, prev_hts;
0464     u32 ae_low, ae_high, ae_target;
0465
0466     bool pending_mode_change;
0467     bool streaming;
0468 };
0469
0470 static inline struct ov5640_dev *to_ov5640_dev(struct v4l2_subdev *sd)
0471 {
0472     return container_of(sd, struct ov5640_dev, sd);
0473 }
0474
0475 static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
0476 {
0477     return &container_of(ctrl->handler, struct ov5640_dev,
0478                  ctrls.handler)->sd;
0479 }
0480
0481 static inline bool ov5640_is_csi2(const struct ov5640_dev *sensor)
0482 {
0483     return sensor->ep.bus_type == V4L2_MBUS_CSI2_DPHY;
0484 }
0485
0486 static inline const struct ov5640_pixfmt *
0487 ov5640_formats(struct ov5640_dev *sensor)
0488 {
0489     return ov5640_is_csi2(sensor) ? ov5640_csi2_formats
0490                       : ov5640_dvp_formats;
0491 }
0492
0493 static const struct ov5640_pixfmt *
0494 ov5640_code_to_pixfmt(struct ov5640_dev *sensor, u32 code)
0495 {
0496     const struct ov5640_pixfmt *formats = ov5640_formats(sensor);
0497     unsigned int i;
0498
0499     for (i = 0; formats[i].code; ++i) {
0500         if (formats[i].code == code)
0501             return &formats[i];
0502     }
0503
0504     return &formats[0];
0505 }
0506
0507 static u32 ov5640_code_to_bpp(struct ov5640_dev *sensor, u32 code)
0508 {
0509     const struct ov5640_pixfmt *format = ov5640_code_to_pixfmt(sensor,
0510                                    code);
0511
0512     return format->bpp;
0513 }
0514
0515 /*
0516  * FIXME: all of these register tables are likely filled with
0517  * entries that set the register to their power-on default values,
0518  * and which are otherwise not touched by this driver. Those entries
0519  * should be identified and removed to speed register load time
0520  * over i2c.
0521  */
0522 /* YUV422 UYVY VGA@30fps */
0523
0524 static const struct v4l2_mbus_framefmt ov5640_default_fmt = {
0525     .code = MEDIA_BUS_FMT_UYVY8_2X8,
0526     .width = 640,
0527     .height = 480,
0528     .colorspace = V4L2_COLORSPACE_SRGB,
0529     .ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(V4L2_COLORSPACE_SRGB),
0530     .quantization = V4L2_QUANTIZATION_FULL_RANGE,
0531     .xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(V4L2_COLORSPACE_SRGB),
0532     .field = V4L2_FIELD_NONE,
0533 };
0534
0535 static const struct reg_value ov5640_init_setting[] = {
0536     {0x3103, 0x11, 0, 0}, {0x3008, 0x82, 0, 5}, {0x3008, 0x42, 0, 0},
0537     {0x3103, 0x03, 0, 0}, {0x3630, 0x36, 0, 0},
0538     {0x3631, 0x0e, 0, 0}, {0x3632, 0xe2, 0, 0}, {0x3633, 0x12, 0, 0},
0539     {0x3621, 0xe0, 0, 0}, {0x3704, 0xa0, 0, 0}, {0x3703, 0x5a, 0, 0},
0540     {0x3715, 0x78, 0, 0}, {0x3717, 0x01, 0, 0}, {0x370b, 0x60, 0, 0},
0541     {0x3705, 0x1a, 0, 0}, {0x3905, 0x02, 0, 0}, {0x3906, 0x10, 0, 0},
0542     {0x3901, 0x0a, 0, 0}, {0x3731, 0x12, 0, 0}, {0x3600, 0x08, 0, 0},
0543     {0x3601, 0x33, 0, 0}, {0x302d, 0x60, 0, 0}, {0x3620, 0x52, 0, 0},
0544     {0x371b, 0x20, 0, 0}, {0x471c, 0x50, 0, 0}, {0x3a13, 0x43, 0, 0},
0545     {0x3a18, 0x00, 0, 0}, {0x3a19, 0xf8, 0, 0}, {0x3635, 0x13, 0, 0},
0546     {0x3636, 0x03, 0, 0}, {0x3634, 0x40, 0, 0}, {0x3622, 0x01, 0, 0},
0547     {0x3c01, 0xa4, 0, 0}, {0x3c04, 0x28, 0, 0}, {0x3c05, 0x98, 0, 0},
0548     {0x3c06, 0x00, 0, 0}, {0x3c07, 0x08, 0, 0}, {0x3c08, 0x00, 0, 0},
0549     {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
0550     {0x3820, 0x41, 0, 0}, {0x3821, 0x07, 0, 0}, {0x3814, 0x31, 0, 0},
0551     {0x3815, 0x31, 0, 0},
0552     {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
0553     {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
0554     {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
0555     {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
0556     {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
0557     {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0}, {0x3000, 0x00, 0, 0},
0558     {0x3002, 0x1c, 0, 0}, {0x3004, 0xff, 0, 0}, {0x3006, 0xc3, 0, 0},
0559     {0x302e, 0x08, 0, 0}, {0x4300, 0x3f, 0, 0},
0560     {0x501f, 0x00, 0, 0}, {0x4407, 0x04, 0, 0},
0561     {0x440e, 0x00, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
0562     {0x4837, 0x0a, 0, 0}, {0x3824, 0x02, 0, 0},
0563     {0x5000, 0xa7, 0, 0}, {0x5001, 0xa3, 0, 0}, {0x5180, 0xff, 0, 0},
0564     {0x5181, 0xf2, 0, 0}, {0x5182, 0x00, 0, 0}, {0x5183, 0x14, 0, 0},
0565     {0x5184, 0x25, 0, 0}, {0x5185, 0x24, 0, 0}, {0x5186, 0x09, 0, 0},
0566     {0x5187, 0x09, 0, 0}, {0x5188, 0x09, 0, 0}, {0x5189, 0x88, 0, 0},
0567     {0x518a, 0x54, 0, 0}, {0x518b, 0xee, 0, 0}, {0x518c, 0xb2, 0, 0},
0568     {0x518d, 0x50, 0, 0}, {0x518e, 0x34, 0, 0}, {0x518f, 0x6b, 0, 0},
0569     {0x5190, 0x46, 0, 0}, {0x5191, 0xf8, 0, 0}, {0x5192, 0x04, 0, 0},
0570     {0x5193, 0x70, 0, 0}, {0x5194, 0xf0, 0, 0}, {0x5195, 0xf0, 0, 0},
0571     {0x5196, 0x03, 0, 0}, {0x5197, 0x01, 0, 0}, {0x5198, 0x04, 0, 0},
0572     {0x5199, 0x6c, 0, 0}, {0x519a, 0x04, 0, 0}, {0x519b, 0x00, 0, 0},
0573     {0x519c, 0x09, 0, 0}, {0x519d, 0x2b, 0, 0}, {0x519e, 0x38, 0, 0},
0574     {0x5381, 0x1e, 0, 0}, {0x5382, 0x5b, 0, 0}, {0x5383, 0x08, 0, 0},
0575     {0x5384, 0x0a, 0, 0}, {0x5385, 0x7e, 0, 0}, {0x5386, 0x88, 0, 0},
0576     {0x5387, 0x7c, 0, 0}, {0x5388, 0x6c, 0, 0}, {0x5389, 0x10, 0, 0},
0577     {0x538a, 0x01, 0, 0}, {0x538b, 0x98, 0, 0}, {0x5300, 0x08, 0, 0},
0578     {0x5301, 0x30, 0, 0}, {0x5302, 0x10, 0, 0}, {0x5303, 0x00, 0, 0},
0579     {0x5304, 0x08, 0, 0}, {0x5305, 0x30, 0, 0}, {0x5306, 0x08, 0, 0},
0580     {0x5307, 0x16, 0, 0}, {0x5309, 0x08, 0, 0}, {0x530a, 0x30, 0, 0},
0581     {0x530b, 0x04, 0, 0}, {0x530c, 0x06, 0, 0}, {0x5480, 0x01, 0, 0},
0582     {0x5481, 0x08, 0, 0}, {0x5482, 0x14, 0, 0}, {0x5483, 0x28, 0, 0},
0583     {0x5484, 0x51, 0, 0}, {0x5485, 0x65, 0, 0}, {0x5486, 0x71, 0, 0},
0584     {0x5487, 0x7d, 0, 0}, {0x5488, 0x87, 0, 0}, {0x5489, 0x91, 0, 0},
0585     {0x548a, 0x9a, 0, 0}, {0x548b, 0xaa, 0, 0}, {0x548c, 0xb8, 0, 0},
0586     {0x548d, 0xcd, 0, 0}, {0x548e, 0xdd, 0, 0}, {0x548f, 0xea, 0, 0},
0587     {0x5490, 0x1d, 0, 0}, {0x5580, 0x02, 0, 0}, {0x5583, 0x40, 0, 0},
0588     {0x5584, 0x10, 0, 0}, {0x5589, 0x10, 0, 0}, {0x558a, 0x00, 0, 0},
0589     {0x558b, 0xf8, 0, 0}, {0x5800, 0x23, 0, 0}, {0x5801, 0x14, 0, 0},
0590     {0x5802, 0x0f, 0, 0}, {0x5803, 0x0f, 0, 0}, {0x5804, 0x12, 0, 0},
0591     {0x5805, 0x26, 0, 0}, {0x5806, 0x0c, 0, 0}, {0x5807, 0x08, 0, 0},
0592     {0x5808, 0x05, 0, 0}, {0x5809, 0x05, 0, 0}, {0x580a, 0x08, 0, 0},
0593     {0x580b, 0x0d, 0, 0}, {0x580c, 0x08, 0, 0}, {0x580d, 0x03, 0, 0},
0594     {0x580e, 0x00, 0, 0}, {0x580f, 0x00, 0, 0}, {0x5810, 0x03, 0, 0},
0595     {0x5811, 0x09, 0, 0}, {0x5812, 0x07, 0, 0}, {0x5813, 0x03, 0, 0},
0596     {0x5814, 0x00, 0, 0}, {0x5815, 0x01, 0, 0}, {0x5816, 0x03, 0, 0},
0597     {0x5817, 0x08, 0, 0}, {0x5818, 0x0d, 0, 0}, {0x5819, 0x08, 0, 0},
0598     {0x581a, 0x05, 0, 0}, {0x581b, 0x06, 0, 0}, {0x581c, 0x08, 0, 0},
0599     {0x581d, 0x0e, 0, 0}, {0x581e, 0x29, 0, 0}, {0x581f, 0x17, 0, 0},
0600     {0x5820, 0x11, 0, 0}, {0x5821, 0x11, 0, 0}, {0x5822, 0x15, 0, 0},
0601     {0x5823, 0x28, 0, 0}, {0x5824, 0x46, 0, 0}, {0x5825, 0x26, 0, 0},
0602     {0x5826, 0x08, 0, 0}, {0x5827, 0x26, 0, 0}, {0x5828, 0x64, 0, 0},
0603     {0x5829, 0x26, 0, 0}, {0x582a, 0x24, 0, 0}, {0x582b, 0x22, 0, 0},
0604     {0x582c, 0x24, 0, 0}, {0x582d, 0x24, 0, 0}, {0x582e, 0x06, 0, 0},
0605     {0x582f, 0x22, 0, 0}, {0x5830, 0x40, 0, 0}, {0x5831, 0x42, 0, 0},
0606     {0x5832, 0x24, 0, 0}, {0x5833, 0x26, 0, 0}, {0x5834, 0x24, 0, 0},
0607     {0x5835, 0x22, 0, 0}, {0x5836, 0x22, 0, 0}, {0x5837, 0x26, 0, 0},
0608     {0x5838, 0x44, 0, 0}, {0x5839, 0x24, 0, 0}, {0x583a, 0x26, 0, 0},
0609     {0x583b, 0x28, 0, 0}, {0x583c, 0x42, 0, 0}, {0x583d, 0xce, 0, 0},
0610     {0x5025, 0x00, 0, 0}, {0x3a0f, 0x30, 0, 0}, {0x3a10, 0x28, 0, 0},
0611     {0x3a1b, 0x30, 0, 0}, {0x3a1e, 0x26, 0, 0}, {0x3a11, 0x60, 0, 0},
0612     {0x3a1f, 0x14, 0, 0}, {0x3008, 0x02, 0, 0}, {0x3c00, 0x04, 0, 300},
0613 };
0614
0615 static const struct reg_value ov5640_setting_low_res[] = {
0616     {0x3c07, 0x08, 0, 0},
0617     {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
0618     {0x3814, 0x31, 0, 0},
0619     {0x3815, 0x31, 0, 0},
0620     {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
0621     {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x03, 0, 0},
0622     {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
0623     {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
0624     {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
0625     {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
0626     {0x4407, 0x04, 0, 0}, {0x5001, 0xa3, 0, 0},
0627 };
0628
0629 static const struct reg_value ov5640_setting_720P_1280_720[] = {
0630     {0x3c07, 0x07, 0, 0},
0631     {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
0632     {0x3814, 0x31, 0, 0},
0633     {0x3815, 0x31, 0, 0},
0634     {0x3618, 0x00, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x64, 0, 0},
0635     {0x3709, 0x52, 0, 0}, {0x370c, 0x03, 0, 0}, {0x3a02, 0x02, 0, 0},
0636     {0x3a03, 0xe4, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0xbc, 0, 0},
0637     {0x3a0a, 0x01, 0, 0}, {0x3a0b, 0x72, 0, 0}, {0x3a0e, 0x01, 0, 0},
0638     {0x3a0d, 0x02, 0, 0}, {0x3a14, 0x02, 0, 0}, {0x3a15, 0xe4, 0, 0},
0639     {0x4001, 0x02, 0, 0}, {0x4004, 0x02, 0, 0},
0640     {0x4407, 0x04, 0, 0}, {0x460b, 0x37, 0, 0}, {0x460c, 0x20, 0, 0},
0641     {0x3824, 0x04, 0, 0}, {0x5001, 0x83, 0, 0},
0642 };
0643
0644 static const struct reg_value ov5640_setting_1080P_1920_1080[] = {
0645     {0x3c07, 0x08, 0, 0},
0646     {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
0647     {0x3814, 0x11, 0, 0},
0648     {0x3815, 0x11, 0, 0},
0649     {0x3618, 0x04, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x21, 0, 0},
0650     {0x3709, 0x12, 0, 0}, {0x370c, 0x00, 0, 0}, {0x3a02, 0x03, 0, 0},
0651     {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
0652     {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
0653     {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
0654     {0x4001, 0x02, 0, 0}, {0x4004, 0x06, 0, 0},
0655     {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
0656     {0x3824, 0x02, 0, 0}, {0x5001, 0x83, 0, 0},
0657     {0x3c07, 0x07, 0, 0}, {0x3c08, 0x00, 0, 0},
0658     {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
0659     {0x3612, 0x2b, 0, 0}, {0x3708, 0x64, 0, 0},
0660     {0x3a02, 0x04, 0, 0}, {0x3a03, 0x60, 0, 0}, {0x3a08, 0x01, 0, 0},
0661     {0x3a09, 0x50, 0, 0}, {0x3a0a, 0x01, 0, 0}, {0x3a0b, 0x18, 0, 0},
0662     {0x3a0e, 0x03, 0, 0}, {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x04, 0, 0},
0663     {0x3a15, 0x60, 0, 0}, {0x4407, 0x04, 0, 0},
0664     {0x460b, 0x37, 0, 0}, {0x460c, 0x20, 0, 0}, {0x3824, 0x04, 0, 0},
0665     {0x4005, 0x1a, 0, 0},
0666 };
0667
0668 static const struct reg_value ov5640_setting_QSXGA_2592_1944[] = {
0669     {0x3c07, 0x08, 0, 0},
0670     {0x3c09, 0x1c, 0, 0}, {0x3c0a, 0x9c, 0, 0}, {0x3c0b, 0x40, 0, 0},
0671     {0x3814, 0x11, 0, 0},
0672     {0x3815, 0x11, 0, 0},
0673     {0x3618, 0x04, 0, 0}, {0x3612, 0x29, 0, 0}, {0x3708, 0x21, 0, 0},
0674     {0x3709, 0x12, 0, 0}, {0x370c, 0x00, 0, 0}, {0x3a02, 0x03, 0, 0},
0675     {0x3a03, 0xd8, 0, 0}, {0x3a08, 0x01, 0, 0}, {0x3a09, 0x27, 0, 0},
0676     {0x3a0a, 0x00, 0, 0}, {0x3a0b, 0xf6, 0, 0}, {0x3a0e, 0x03, 0, 0},
0677     {0x3a0d, 0x04, 0, 0}, {0x3a14, 0x03, 0, 0}, {0x3a15, 0xd8, 0, 0},
0678     {0x4001, 0x02, 0, 0}, {0x4004, 0x06, 0, 0},
0679     {0x4407, 0x04, 0, 0}, {0x460b, 0x35, 0, 0}, {0x460c, 0x22, 0, 0},
0680     {0x3824, 0x02, 0, 0}, {0x5001, 0x83, 0, 70},
0681 };
0682
0683 static const struct ov5640_mode_info ov5640_mode_data[OV5640_NUM_MODES] = {
0684     {
0685         /* 160x120 */
0686         .id     = OV5640_MODE_QQVGA_160_120,
0687         .dn_mode    = SUBSAMPLING,
0688         .pixel_rate = OV5640_PIXEL_RATE_48M,
0689         .width      = 160,
0690         .height     = 120,
0691         .dvp_timings = {
0692             .analog_crop = {
0693                 .left   = 0,
0694                 .top    = 4,
0695                 .width  = 2624,
0696                 .height = 1944,
0697             },
0698             .crop = {
0699                 .left   = 16,
0700                 .top    = 6,
0701                 .width  = 160,
0702                 .height = 120,
0703             },
0704             .htot       = 1896,
0705             .vblank_def = 864,
0706         },
0707         .csi2_timings = {
0708             /* Feed the full valid pixel array to the ISP. */
0709             .analog_crop = {
0710                 .left   = OV5640_PIXEL_ARRAY_LEFT,
0711                 .top    = OV5640_PIXEL_ARRAY_TOP,
0712                 .width  = OV5640_PIXEL_ARRAY_WIDTH,
0713                 .height = OV5640_PIXEL_ARRAY_HEIGHT,
0714             },
0715             /* Maintain a minimum processing margin. */
0716             .crop = {
0717                 .left   = 2,
0718                 .top    = 4,
0719                 .width  = 160,
0720                 .height = 120,
0721             },
0722             .htot       = 1600,
0723             .vblank_def = 878,
0724         },
0725         .reg_data   = ov5640_setting_low_res,
0726         .reg_data_size  = ARRAY_SIZE(ov5640_setting_low_res),
0727         .max_fps    = OV5640_30_FPS,
0728         .def_fps    = OV5640_30_FPS
0729     }, {
0730         /* 176x144 */
0731         .id     = OV5640_MODE_QCIF_176_144,
0732         .dn_mode    = SUBSAMPLING,
0733         .pixel_rate = OV5640_PIXEL_RATE_48M,
0734         .width      = 176,
0735         .height     = 144,
0736         .dvp_timings = {
0737             .analog_crop = {
0738                 .left   = 0,
0739                 .top    = 4,
0740                 .width  = 2624,
0741                 .height = 1944,
0742             },
0743             .crop = {
0744                 .left   = 16,
0745                 .top    = 6,
0746                 .width  = 176,
0747                 .height = 144,
0748             },
0749             .htot       = 1896,
0750             .vblank_def = 840,
0751         },
0752         .csi2_timings = {
0753             /* Feed the full valid pixel array to the ISP. */
0754             .analog_crop = {
0755                 .left   = OV5640_PIXEL_ARRAY_LEFT,
0756                 .top    = OV5640_PIXEL_ARRAY_TOP,
0757                 .width  = OV5640_PIXEL_ARRAY_WIDTH,
0758                 .height = OV5640_PIXEL_ARRAY_HEIGHT,
0759             },
0760             /* Maintain a minimum processing margin. */
0761             .crop = {
0762                 .left   = 2,
0763                 .top    = 4,
0764                 .width  = 176,
0765                 .height = 144,
0766             },
0767             .htot       = 1600,
0768             .vblank_def = 854,
0769         },
0770         .reg_data   = ov5640_setting_low_res,
0771         .reg_data_size  = ARRAY_SIZE(ov5640_setting_low_res),
0772         .max_fps    = OV5640_30_FPS,
0773         .def_fps    = OV5640_30_FPS
0774     }, {
0775         /* 320x240 */
0776         .id     = OV5640_MODE_QVGA_320_240,
0777         .dn_mode    = SUBSAMPLING,
0778         .width      = 320,
0779         .height     = 240,
0780         .pixel_rate = OV5640_PIXEL_RATE_48M,
0781         .dvp_timings = {
0782             .analog_crop = {
0783                 .left   = 0,
0784                 .top    = 4,
0785                 .width  = 2624,
0786                 .height = 1944,
0787             },
0788             .crop = {
0789                 .left   = 16,
0790                 .top    = 6,
0791                 .width  = 320,
0792                 .height = 240,
0793             },
0794             .htot       = 1896,
0795             .vblank_def = 744,
0796         },
0797         .csi2_timings = {
0798             /* Feed the full valid pixel array to the ISP. */
0799             .analog_crop = {
0800                 .left   = OV5640_PIXEL_ARRAY_LEFT,
0801                 .top    = OV5640_PIXEL_ARRAY_TOP,
0802                 .width  = OV5640_PIXEL_ARRAY_WIDTH,
0803                 .height = OV5640_PIXEL_ARRAY_HEIGHT,
0804             },
0805             /* Maintain a minimum processing margin. */
0806             .crop = {
0807                 .left   = 2,
0808                 .top    = 4,
0809                 .width  = 320,
0810                 .height = 240,
0811             },
0812             .htot       = 1600,
0813             .vblank_def = 760,
0814         },
0815         .reg_data   = ov5640_setting_low_res,
0816         .reg_data_size  = ARRAY_SIZE(ov5640_setting_low_res),
0817         .max_fps    = OV5640_30_FPS,
0818         .def_fps    = OV5640_30_FPS
0819     }, {
0820         /* 640x480 */
0821         .id     = OV5640_MODE_VGA_640_480,
0822         .dn_mode    = SUBSAMPLING,
0823         .pixel_rate = OV5640_PIXEL_RATE_48M,
0824         .width      = 640,
0825         .height     = 480,
0826         .dvp_timings = {
0827             .analog_crop = {
0828                 .left   = 0,
0829                 .top    = 4,
0830                 .width  = 2624,
0831                 .height = 1944,
0832             },
0833             .crop = {
0834                 .left   = 16,
0835                 .top    = 6,
0836                 .width  = 640,
0837                 .height = 480,
0838             },
0839             .htot       = 1896,
0840             .vblank_def = 600,
0841         },
0842         .csi2_timings = {
0843             /* Feed the full valid pixel array to the ISP. */
0844             .analog_crop = {
0845                 .left   = OV5640_PIXEL_ARRAY_LEFT,
0846                 .top    = OV5640_PIXEL_ARRAY_TOP,
0847                 .width  = OV5640_PIXEL_ARRAY_WIDTH,
0848                 .height = OV5640_PIXEL_ARRAY_HEIGHT,
0849             },
0850             /* Maintain a minimum processing margin. */
0851             .crop = {
0852                 .left   = 2,
0853                 .top    = 4,
0854                 .width  = 640,
0855                 .height = 480,
0856             },
0857             .htot       = 1600,
0858             .vblank_def = 520,
0859         },
0860         .reg_data   = ov5640_setting_low_res,
0861         .reg_data_size  = ARRAY_SIZE(ov5640_setting_low_res),
0862         .max_fps    = OV5640_60_FPS,
0863         .def_fps    = OV5640_30_FPS
0864     }, {
0865         /* 720x480 */
0866         .id     = OV5640_MODE_NTSC_720_480,
0867         .dn_mode    = SUBSAMPLING,
0868         .width      = 720,
0869         .height     = 480,
0870         .pixel_rate = OV5640_PIXEL_RATE_96M,
0871         .dvp_timings = {
0872             .analog_crop = {
0873                 .left   = 0,
0874                 .top    = 4,
0875                 .width  = 2624,
0876                 .height = 1944,
0877             },
0878             .crop = {
0879                 .left   = 56,
0880                 .top    = 60,
0881                 .width  = 720,
0882                 .height = 480,
0883             },
0884             .htot       = 1896,
0885             .vblank_def = 504,
0886         },
0887         .csi2_timings = {
0888             /* Feed the full valid pixel array to the ISP. */
0889             .analog_crop = {
0890                 .left   = OV5640_PIXEL_ARRAY_LEFT,
0891                 .top    = OV5640_PIXEL_ARRAY_TOP,
0892                 .width  = OV5640_PIXEL_ARRAY_WIDTH,
0893                 .height = OV5640_PIXEL_ARRAY_HEIGHT,
0894             },
0895             .crop = {
0896                 .left   = 56,
0897                 .top    = 60,
0898                 .width  = 720,
0899                 .height = 480,
0900             },
0901             .htot       = 1896,
0902             .vblank_def = 1206,
0903         },
0904         .reg_data   = ov5640_setting_low_res,
0905         .reg_data_size  = ARRAY_SIZE(ov5640_setting_low_res),
0906         .max_fps    = OV5640_30_FPS,
0907         .def_fps    = OV5640_30_FPS
0908     }, {
0909         /* 720x576 */
0910         .id     = OV5640_MODE_PAL_720_576,
0911         .dn_mode    = SUBSAMPLING,
0912         .width      = 720,
0913         .height     = 576,
0914         .pixel_rate = OV5640_PIXEL_RATE_96M,
0915         .dvp_timings = {
0916             .analog_crop = {
0917                 .left   = 0,
0918                 .top    = 4,
0919                 .width  = 2624,
0920                 .height = 1944,
0921             },
0922             .crop = {
0923                 .left   = 56,
0924                 .top    = 6,
0925                 .width  = 720,
0926                 .height = 576,
0927             },
0928             .htot       = 1896,
0929             .vblank_def = 408,
0930         },
0931         .csi2_timings = {
0932             /* Feed the full valid pixel array to the ISP. */
0933             .analog_crop = {
0934                 .left   = OV5640_PIXEL_ARRAY_LEFT,
0935                 .top    = OV5640_PIXEL_ARRAY_TOP,
0936                 .width  = OV5640_PIXEL_ARRAY_WIDTH,
0937                 .height = OV5640_PIXEL_ARRAY_HEIGHT,
0938             },
0939             .crop = {
0940                 .left   = 56,
0941                 .top    = 6,
0942                 .width  = 720,
0943                 .height = 576,
0944             },
0945             .htot       = 1896,
0946             .vblank_def = 1110,
0947         },
0948         .reg_data   = ov5640_setting_low_res,
0949         .reg_data_size  = ARRAY_SIZE(ov5640_setting_low_res),
0950         .max_fps    = OV5640_30_FPS,
0951         .def_fps    = OV5640_30_FPS
0952     }, {
0953         /* 1024x768 */
0954         .id     = OV5640_MODE_XGA_1024_768,
0955         .dn_mode    = SUBSAMPLING,
0956         .pixel_rate = OV5640_PIXEL_RATE_96M,
0957         .width      = 1024,
0958         .height     = 768,
0959         .dvp_timings = {
0960             .analog_crop = {
0961                 .left   = 0,
0962                 .top    = 4,
0963                 .width  = 2624,
0964                 .height = 1944,
0965             },
0966             .crop = {
0967                 .left   = 16,
0968                 .top    = 6,
0969                 .width  = 1024,
0970                 .height = 768,
0971             },
0972             .htot       = 1896,
0973             .vblank_def = 312,
0974         },
0975         .csi2_timings = {
0976             .analog_crop = {
0977                 .left   = 0,
0978                 .top    = 4,
0979                 .width  = OV5640_NATIVE_WIDTH,
0980                 .height = OV5640_PIXEL_ARRAY_HEIGHT,
0981             },
0982             .crop = {
0983                 .left   = 16,
0984                 .top    = 6,
0985                 .width  = 1024,
0986                 .height = 768,
0987             },
0988             .htot       = 1896,
0989             .vblank_def = 918,
0990         },
0991         .reg_data   = ov5640_setting_low_res,
0992         .reg_data_size  = ARRAY_SIZE(ov5640_setting_low_res),
0993         .max_fps    = OV5640_30_FPS,
0994         .def_fps    = OV5640_30_FPS
0995     }, {
0996         /* 1280x720 */
0997         .id     = OV5640_MODE_720P_1280_720,
0998         .dn_mode    = SUBSAMPLING,
0999         .pixel_rate = OV5640_PIXEL_RATE_124M,
1000         .width      = 1280,
1001         .height     = 720,
1002         .dvp_timings = {
1003             .analog_crop = {
1004                 .left   = 0,
1005                 .top    = 250,
1006                 .width  = 2624,
1007                 .height = 1456,
1008             },
1009             .crop = {
1010                 .left   = 16,
1011                 .top    = 4,
1012                 .width  = 1280,
1013                 .height = 720,
1014             },
1015             .htot       = 1892,
1016             .vblank_def = 20,
1017         },
1018         .csi2_timings = {
1019             .analog_crop = {
1020                 .left   = 0,
1021                 .top    = 250,
1022                 .width  = 2624,
1023                 .height = 1456,
1024             },
1025             .crop = {
1026                 .left   = 16,
1027                 .top    = 4,
1028                 .width  = 1280,
1029                 .height = 720,
1030             },
1031             .htot       = 1600,
1032             .vblank_def = 560,
1033         },
1034         .reg_data   = ov5640_setting_720P_1280_720,
1035         .reg_data_size  = ARRAY_SIZE(ov5640_setting_720P_1280_720),
1036         .max_fps    = OV5640_30_FPS,
1037         .def_fps    = OV5640_30_FPS
1038     }, {
1039         /* 1920x1080 */
1040         .id     = OV5640_MODE_1080P_1920_1080,
1041         .dn_mode    = SCALING,
1042         .pixel_rate = OV5640_PIXEL_RATE_148M,
1043         .width      = 1920,
1044         .height     = 1080,
1045         .dvp_timings = {
1046             .analog_crop = {
1047                 .left   = 336,
1048                 .top    = 434,
1049                 .width  = 1952,
1050                 .height = 1088,
1051             },
1052             .crop = {
1053                 .left   = 16,
1054                 .top    = 4,
1055                 .width  = 1920,
1056                 .height = 1080,
1057             },
1058             .htot       = 2500,
1059             .vblank_def = 40,
1060         },
1061         .csi2_timings = {
1062             /* Crop the full valid pixel array in the center. */
1063             .analog_crop = {
1064                 .left   = 336,
1065                 .top    = 434,
1066                 .width  = 1952,
1067                 .height = 1088,
1068             },
1069             /* Maintain a larger processing margins. */
1070             .crop = {
1071                 .left   = 16,
1072                 .top    = 4,
1073                 .width  = 1920,
1074                 .height = 1080,
1075             },
1076             .htot       = 2234,
1077             .vblank_def = 24,
1078         },
1079         .reg_data   = ov5640_setting_1080P_1920_1080,
1080         .reg_data_size  = ARRAY_SIZE(ov5640_setting_1080P_1920_1080),
1081         .max_fps    = OV5640_30_FPS,
1082         .def_fps    = OV5640_30_FPS
1083     }, {
1084         /* 2592x1944 */
1085         .id     = OV5640_MODE_QSXGA_2592_1944,
1086         .dn_mode    = SCALING,
1087         .pixel_rate = OV5640_PIXEL_RATE_168M,
1088         .width      = OV5640_PIXEL_ARRAY_WIDTH,
1089         .height     = OV5640_PIXEL_ARRAY_HEIGHT,
1090         .dvp_timings = {
1091             .analog_crop = {
1092                 .left   = 0,
1093                 .top    = 0,
1094                 .width  = 2624,
1095                 .height = 1952,
1096             },
1097             .crop = {
1098                 .left   = 16,
1099                 .top    = 4,
1100                 .width  = 2592,
1101                 .height = 1944,
1102             },
1103             .htot       = 2844,
1104             .vblank_def = 24,
1105         },
1106         .csi2_timings = {
1107             /* Give more processing margin to full resolution. */
1108             .analog_crop = {
1109                 .left   = 0,
1110                 .top    = 0,
1111                 .width  = OV5640_NATIVE_WIDTH,
1112                 .height = 1952,
1113             },
1114             .crop = {
1115                 .left   = 16,
1116                 .top    = 4,
1117                 .width  = 2592,
1118                 .height = 1944,
1119             },
1120             .htot       = 2844,
1121             .vblank_def = 24,
1122         },
1123         .reg_data   = ov5640_setting_QSXGA_2592_1944,
1124         .reg_data_size  = ARRAY_SIZE(ov5640_setting_QSXGA_2592_1944),
1125         .max_fps    = OV5640_15_FPS,
1126         .def_fps    = OV5640_15_FPS
1127     },
1128 };
1129
1130 static const struct ov5640_timings *
1131 ov5640_timings(const struct ov5640_dev *sensor,
1132            const struct ov5640_mode_info *mode)
1133 {
1134     if (ov5640_is_csi2(sensor))
1135         return &mode->csi2_timings;
1136
1137     return &mode->dvp_timings;
1138 }
1139
1140 static int ov5640_init_slave_id(struct ov5640_dev *sensor)
1141 {
1142     struct i2c_client *client = sensor->i2c_client;
1143     struct i2c_msg msg;
1144     u8 buf[3];
1145     int ret;
1146
1147     if (client->addr == OV5640_DEFAULT_SLAVE_ID)
1148         return 0;
1149
1150     buf[0] = OV5640_REG_SLAVE_ID >> 8;
1151     buf[1] = OV5640_REG_SLAVE_ID & 0xff;
1152     buf[2] = client->addr << 1;
1153
1154     msg.addr = OV5640_DEFAULT_SLAVE_ID;
1155     msg.flags = 0;
1156     msg.buf = buf;
1157     msg.len = sizeof(buf);
1158
1159     ret = i2c_transfer(client->adapter, &msg, 1);
1160     if (ret < 0) {
1161         dev_err(&client->dev, "%s: failed with %d\n", __func__, ret);
1162         return ret;
1163     }
1164
1165     return 0;
1166 }
1167
1168 static int ov5640_write_reg(struct ov5640_dev *sensor, u16 reg, u8 val)
1169 {
1170     struct i2c_client *client = sensor->i2c_client;
1171     struct i2c_msg msg;
1172     u8 buf[3];
1173     int ret;
1174
1175     buf[0] = reg >> 8;
1176     buf[1] = reg & 0xff;
1177     buf[2] = val;
1178
1179     msg.addr = client->addr;
1180     msg.flags = client->flags;
1181     msg.buf = buf;
1182     msg.len = sizeof(buf);
1183
1184     ret = i2c_transfer(client->adapter, &msg, 1);
1185     if (ret < 0) {
1186         dev_err(&client->dev, "%s: error: reg=%x, val=%x\n",
1187             __func__, reg, val);
1188         return ret;
1189     }
1190
1191     return 0;
1192 }
1193
1194 static int ov5640_read_reg(struct ov5640_dev *sensor, u16 reg, u8 *val)
1195 {
1196     struct i2c_client *client = sensor->i2c_client;
1197     struct i2c_msg msg[2];
1198     u8 buf[2];
1199     int ret;
1200
1201     buf[0] = reg >> 8;
1202     buf[1] = reg & 0xff;
1203
1204     msg[0].addr = client->addr;
1205     msg[0].flags = client->flags;
1206     msg[0].buf = buf;
1207     msg[0].len = sizeof(buf);
1208
1209     msg[1].addr = client->addr;
1210     msg[1].flags = client->flags | I2C_M_RD;
1211     msg[1].buf = buf;
1212     msg[1].len = 1;
1213
1214     ret = i2c_transfer(client->adapter, msg, 2);
1215     if (ret < 0) {
1216         dev_err(&client->dev, "%s: error: reg=%x\n",
1217             __func__, reg);
1218         return ret;
1219     }
1220
1221     *val = buf[0];
1222     return 0;
1223 }
1224
1225 static int ov5640_read_reg16(struct ov5640_dev *sensor, u16 reg, u16 *val)
1226 {
1227     u8 hi, lo;
1228     int ret;
1229
1230     ret = ov5640_read_reg(sensor, reg, &hi);
1231     if (ret)
1232         return ret;
1233     ret = ov5640_read_reg(sensor, reg + 1, &lo);
1234     if (ret)
1235         return ret;
1236
1237     *val = ((u16)hi << 8) | (u16)lo;
1238     return 0;
1239 }
1240
1241 static int ov5640_write_reg16(struct ov5640_dev *sensor, u16 reg, u16 val)
1242 {
1243     int ret;
1244
1245     ret = ov5640_write_reg(sensor, reg, val >> 8);
1246     if (ret)
1247         return ret;
1248
1249     return ov5640_write_reg(sensor, reg + 1, val & 0xff);
1250 }
1251
1252 static int ov5640_mod_reg(struct ov5640_dev *sensor, u16 reg,
1253               u8 mask, u8 val)
1254 {
1255     u8 readval;
1256     int ret;
1257
1258     ret = ov5640_read_reg(sensor, reg, &readval);
1259     if (ret)
1260         return ret;
1261
1262     readval &= ~mask;
1263     val &= mask;
1264     val |= readval;
1265
1266     return ov5640_write_reg(sensor, reg, val);
1267 }
1268
1269 /*
1270  * After trying the various combinations, reading various
1271  * documentations spread around the net, and from the various
1272  * feedback, the clock tree is probably as follows:
1273  *
1274  *   +--------------+
1275  *   |  Ext. Clock  |
1276  *   +-+------------+
1277  *     |  +----------+
1278  *     +->|   PLL1   | - reg 0x3036, for the multiplier
1279  *        +-+--------+ - reg 0x3037, bits 0-3 for the pre-divider
1280  *          |  +--------------+
1281  *          +->| System Clock |  - reg 0x3035, bits 4-7
1282  *             +-+------------+
1283  *               |  +--------------+
1284  *               +->| MIPI Divider | - reg 0x3035, bits 0-3
1285  *               |  +-+------------+
1286  *               |    +----------------> MIPI SCLK
1287  *               |    +  +-----+
1288  *               |    +->| / 2 |-------> MIPI BIT CLK
1289  *               |       +-----+
1290  *               |  +--------------+
1291  *               +->| PLL Root Div | - reg 0x3037, bit 4
1292  *                  +-+------------+
1293  *                    |  +---------+
1294  *                    +->| Bit Div | - reg 0x3034, bits 0-3
1295  *                       +-+-------+
1296  *                         |  +-------------+
1297  *                         +->| SCLK Div    | - reg 0x3108, bits 0-1
1298  *                         |  +-+-----------+
1299  *                         |    +---------------> SCLK
1300  *                         |  +-------------+
1301  *                         +->| SCLK 2X Div | - reg 0x3108, bits 2-3
1302  *                         |  +-+-----------+
1303  *                         |    +---------------> SCLK 2X
1304  *                         |  +-------------+
1305  *                         +->| PCLK Div    | - reg 0x3108, bits 4-5
1306  *                            ++------------+
1307  *                             +  +-----------+
1308  *                             +->|   P_DIV   | - reg 0x3035, bits 0-3
1309  *                                +-----+-----+
1310  *                                       +------------> PCLK
1311  *
1312  * There seems to be also constraints:
1313  *  - the PLL pre-divider output rate should be in the 4-27MHz range
1314  *  - the PLL multiplier output rate should be in the 500-1000MHz range
1315  *  - PCLK >= SCLK * 2 in YUV, >= SCLK in Raw or JPEG
1316  */
1317
1318 /*
1319  * This is supposed to be ranging from 1 to 8, but the value is always
1320  * set to 3 in the vendor kernels.
1321  */
1322 #define OV5640_PLL_PREDIV   3
1323
1324 #define OV5640_PLL_MULT_MIN 4
1325 #define OV5640_PLL_MULT_MAX 252
1326
1327 /*
1328  * This is supposed to be ranging from 1 to 16, but the value is
1329  * always set to either 1 or 2 in the vendor kernels.
1330  */
1331 #define OV5640_SYSDIV_MIN   1
1332 #define OV5640_SYSDIV_MAX   16
1333
1334 /*
1335  * This is supposed to be ranging from 1 to 2, but the value is always
1336  * set to 2 in the vendor kernels.
1337  */
1338 #define OV5640_PLL_ROOT_DIV         2
1339 #define OV5640_PLL_CTRL3_PLL_ROOT_DIV_2     BIT(4)
1340
1341 /*
1342  * We only supports 8-bit formats at the moment
1343  */
1344 #define OV5640_BIT_DIV              2
1345 #define OV5640_PLL_CTRL0_MIPI_MODE_8BIT     0x08
1346
1347 /*
1348  * This is supposed to be ranging from 1 to 8, but the value is always
1349  * set to 2 in the vendor kernels.
1350  */
1351 #define OV5640_SCLK_ROOT_DIV    2
1352
1353 /*
1354  * This is hardcoded so that the consistency is maintained between SCLK and
1355  * SCLK 2x.
1356  */
1357 #define OV5640_SCLK2X_ROOT_DIV (OV5640_SCLK_ROOT_DIV / 2)
1358
1359 /*
1360  * This is supposed to be ranging from 1 to 8, but the value is always
1361  * set to 1 in the vendor kernels.
1362  */
1363 #define OV5640_PCLK_ROOT_DIV            1
1364 #define OV5640_PLL_SYS_ROOT_DIVIDER_BYPASS  0x00
1365
1366 static unsigned long ov5640_compute_sys_clk(struct ov5640_dev *sensor,
1367                         u8 pll_prediv, u8 pll_mult,
1368                         u8 sysdiv)
1369 {
1370     unsigned long sysclk = sensor->xclk_freq / pll_prediv * pll_mult;
1371
1372     /* PLL1 output cannot exceed 1GHz. */
1373     if (sysclk / 1000000 > 1000)
1374         return 0;
1375
1376     return sysclk / sysdiv;
1377 }
1378
1379 static unsigned long ov5640_calc_sys_clk(struct ov5640_dev *sensor,
1380                      unsigned long rate,
1381                      u8 *pll_prediv, u8 *pll_mult,
1382                      u8 *sysdiv)
1383 {
1384     unsigned long best = ~0;
1385     u8 best_sysdiv = 1, best_mult = 1;
1386     u8 _sysdiv, _pll_mult;
1387
1388     for (_sysdiv = OV5640_SYSDIV_MIN;
1389          _sysdiv <= OV5640_SYSDIV_MAX;
1390          _sysdiv++) {
1391         for (_pll_mult = OV5640_PLL_MULT_MIN;
1392              _pll_mult <= OV5640_PLL_MULT_MAX;
1393              _pll_mult++) {
1394             unsigned long _rate;
1395
1396             /*
1397              * The PLL multiplier cannot be odd if above
1398              * 127.
1399              */
1400             if (_pll_mult > 127 && (_pll_mult % 2))
1401                 continue;
1402
1403             _rate = ov5640_compute_sys_clk(sensor,
1404                                OV5640_PLL_PREDIV,
1405                                _pll_mult, _sysdiv);
1406
1407             /*
1408              * We have reached the maximum allowed PLL1 output,
1409              * increase sysdiv.
1410              */
1411             if (!_rate)
1412                 break;
1413
1414             /*
1415              * Prefer rates above the expected clock rate than
1416              * below, even if that means being less precise.
1417              */
1418             if (_rate < rate)
1419                 continue;
1420
1421             if (abs(rate - _rate) < abs(rate - best)) {
1422                 best = _rate;
1423                 best_sysdiv = _sysdiv;
1424                 best_mult = _pll_mult;
1425             }
1426
1427             if (_rate == rate)
1428                 goto out;
1429         }
1430     }
1431
1432 out:
1433     *sysdiv = best_sysdiv;
1434     *pll_prediv = OV5640_PLL_PREDIV;
1435     *pll_mult = best_mult;
1436
1437     return best;
1438 }
1439
1440 /*
1441  * ov5640_set_mipi_pclk() - Calculate the clock tree configuration values
1442  *              for the MIPI CSI-2 output.
1443  */
1444 static int ov5640_set_mipi_pclk(struct ov5640_dev *sensor)
1445 {
1446     u8 bit_div, mipi_div, pclk_div, sclk_div, sclk2x_div, root_div;
1447     u8 prediv, mult, sysdiv;
1448     unsigned long link_freq;
1449     unsigned long sysclk;
1450     u8 pclk_period;
1451     u32 sample_rate;
1452     u32 num_lanes;
1453     int ret;
1454
1455     /* Use the link freq computed at ov5640_update_pixel_rate() time. */
1456     link_freq = sensor->current_link_freq;
1457
1458     /*
1459      * - mipi_div - Additional divider for the MIPI lane clock.
1460      *
1461      * Higher link frequencies would make sysclk > 1GHz.
1462      * Keep the sysclk low and do not divide in the MIPI domain.
1463      */
1464     if (link_freq > OV5640_LINK_RATE_MAX)
1465         mipi_div = 1;
1466     else
1467         mipi_div = 2;
1468
1469     sysclk = link_freq * mipi_div;
1470     ov5640_calc_sys_clk(sensor, sysclk, &prediv, &mult, &sysdiv);
1471
1472     /*
1473      * Adjust PLL parameters to maintain the MIPI_SCLK-to-PCLK ratio.
1474      *
1475      * - root_div = 2 (fixed)
1476      * - bit_div : MIPI 8-bit = 2; MIPI 10-bit = 2.5
1477      * - pclk_div = 1 (fixed)
1478      * - p_div  = (2 lanes ? mipi_div : 2 * mipi_div)
1479      *
1480      * This results in the following MIPI_SCLK depending on the number
1481      * of lanes:
1482      *
1483      * - 2 lanes: MIPI_SCLK = (4 or 5) * PCLK
1484      * - 1 lanes: MIPI_SCLK = (8 or 10) * PCLK
1485      */
1486     root_div = OV5640_PLL_CTRL3_PLL_ROOT_DIV_2;
1487     bit_div =  OV5640_PLL_CTRL0_MIPI_MODE_8BIT;
1488     pclk_div = ilog2(OV5640_PCLK_ROOT_DIV);
1489
1490     /*
1491      * Scaler clock:
1492      * - YUV: PCLK >= 2 * SCLK
1493      * - RAW or JPEG: PCLK >= SCLK
1494      * - sclk2x_div = sclk_div / 2
1495      */
1496     sclk_div = ilog2(OV5640_SCLK_ROOT_DIV);
1497     sclk2x_div = ilog2(OV5640_SCLK2X_ROOT_DIV);
1498
1499     /*
1500      * Set the pixel clock period expressed in ns with 1-bit decimal
1501      * (0x01=0.5ns).
1502      *
1503      * The register is very briefly documented. In the OV5645 datasheet it
1504      * is described as (2 * pclk period), and from testing it seems the
1505      * actual definition is 2 * 8-bit sample period.
1506      *
1507      * 2 * sample_period = (mipi_clk * 2 * num_lanes / bpp) * (bpp / 8) / 2
1508      */
1509     num_lanes = sensor->ep.bus.mipi_csi2.num_data_lanes;
1510     sample_rate = (link_freq * mipi_div * num_lanes * 2) / 16;
1511     pclk_period = 2000000000UL / sample_rate;
1512
1513     /* Program the clock tree registers. */
1514     ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL0, 0x0f, bit_div);
1515     if (ret)
1516         return ret;
1517
1518     ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL1, 0xff,
1519                  (sysdiv << 4) | mipi_div);
1520     if (ret)
1521         return ret;
1522
1523     ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL2, 0xff, mult);
1524     if (ret)
1525         return ret;
1526
1527     ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL3, 0x1f,
1528                  root_div | prediv);
1529     if (ret)
1530         return ret;
1531
1532     ret = ov5640_mod_reg(sensor, OV5640_REG_SYS_ROOT_DIVIDER, 0x3f,
1533                  (pclk_div << 4) | (sclk2x_div << 2) | sclk_div);
1534     if (ret)
1535         return ret;
1536
1537     return ov5640_write_reg(sensor, OV5640_REG_PCLK_PERIOD, pclk_period);
1538 }
1539
1540 static u32 ov5640_calc_pixel_rate(struct ov5640_dev *sensor)
1541 {
1542     const struct ov5640_mode_info *mode = sensor->current_mode;
1543     const struct ov5640_timings *timings = &mode->dvp_timings;
1544     u32 rate;
1545
1546     rate = timings->htot * (timings->crop.height + timings->vblank_def);
1547     rate *= ov5640_framerates[sensor->current_fr];
1548
1549     return rate;
1550 }
1551
1552 static unsigned long ov5640_calc_pclk(struct ov5640_dev *sensor,
1553                       unsigned long rate,
1554                       u8 *pll_prediv, u8 *pll_mult, u8 *sysdiv,
1555                       u8 *pll_rdiv, u8 *bit_div, u8 *pclk_div)
1556 {
1557     unsigned long _rate = rate * OV5640_PLL_ROOT_DIV * OV5640_BIT_DIV *
1558                 OV5640_PCLK_ROOT_DIV;
1559
1560     _rate = ov5640_calc_sys_clk(sensor, _rate, pll_prediv, pll_mult,
1561                     sysdiv);
1562     *pll_rdiv = OV5640_PLL_ROOT_DIV;
1563     *bit_div = OV5640_BIT_DIV;
1564     *pclk_div = OV5640_PCLK_ROOT_DIV;
1565
1566     return _rate / *pll_rdiv / *bit_div / *pclk_div;
1567 }
1568
1569 static int ov5640_set_dvp_pclk(struct ov5640_dev *sensor)
1570 {
1571     u8 prediv, mult, sysdiv, pll_rdiv, bit_div, pclk_div;
1572     u32 rate;
1573     int ret;
1574
1575     rate = ov5640_calc_pixel_rate(sensor);
1576     rate *= ov5640_code_to_bpp(sensor, sensor->fmt.code);
1577     rate /= sensor->ep.bus.parallel.bus_width;
1578
1579     ov5640_calc_pclk(sensor, rate, &prediv, &mult, &sysdiv, &pll_rdiv,
1580              &bit_div, &pclk_div);
1581
1582     if (bit_div == 2)
1583         bit_div = 8;
1584
1585     ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL0,
1586                  0x0f, bit_div);
1587     if (ret)
1588         return ret;
1589
1590     /*
1591      * We need to set sysdiv according to the clock, and to clear
1592      * the MIPI divider.
1593      */
1594     ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL1,
1595                  0xff, sysdiv << 4);
1596     if (ret)
1597         return ret;
1598
1599     ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL2,
1600                  0xff, mult);
1601     if (ret)
1602         return ret;
1603
1604     ret = ov5640_mod_reg(sensor, OV5640_REG_SC_PLL_CTRL3,
1605                  0x1f, prediv | ((pll_rdiv - 1) << 4));
1606     if (ret)
1607         return ret;
1608
1609     return ov5640_mod_reg(sensor, OV5640_REG_SYS_ROOT_DIVIDER, 0x30,
1610                   (ilog2(pclk_div) << 4));
1611 }
1612
1613 /* set JPEG framing sizes */
1614 static int ov5640_set_jpeg_timings(struct ov5640_dev *sensor,
1615                    const struct ov5640_mode_info *mode)
1616 {
1617     int ret;
1618
1619     /*
1620      * compression mode 3 timing
1621      *
1622      * Data is transmitted with programmable width (VFIFO_HSIZE).
1623      * No padding done. Last line may have less data. Varying
1624      * number of lines per frame, depending on amount of data.
1625      */
1626     ret = ov5640_mod_reg(sensor, OV5640_REG_JPG_MODE_SELECT, 0x7, 0x3);
1627     if (ret < 0)
1628         return ret;
1629
1630     ret = ov5640_write_reg16(sensor, OV5640_REG_VFIFO_HSIZE, mode->width);
1631     if (ret < 0)
1632         return ret;
1633
1634     return ov5640_write_reg16(sensor, OV5640_REG_VFIFO_VSIZE, mode->height);
1635 }
1636
1637 /* download ov5640 settings to sensor through i2c */
1638 static int ov5640_set_timings(struct ov5640_dev *sensor,
1639                   const struct ov5640_mode_info *mode)
1640 {
1641     const struct ov5640_timings *timings;
1642     const struct v4l2_rect *analog_crop;
1643     const struct v4l2_rect *crop;
1644     int ret;
1645
1646     if (sensor->fmt.code == MEDIA_BUS_FMT_JPEG_1X8) {
1647         ret = ov5640_set_jpeg_timings(sensor, mode);
1648         if (ret < 0)
1649             return ret;
1650     }
1651
1652     timings = ov5640_timings(sensor, mode);
1653     analog_crop = &timings->analog_crop;
1654     crop = &timings->crop;
1655
1656     ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_HS,
1657                  analog_crop->left);
1658     if (ret < 0)
1659         return ret;
1660
1661     ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_VS,
1662                  analog_crop->top);
1663     if (ret < 0)
1664         return ret;
1665
1666     ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_HW,
1667                  analog_crop->left + analog_crop->width - 1);
1668     if (ret < 0)
1669         return ret;
1670
1671     ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_VH,
1672                  analog_crop->top + analog_crop->height - 1);
1673     if (ret < 0)
1674         return ret;
1675
1676     ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_HOFFS, crop->left);
1677     if (ret < 0)
1678         return ret;
1679
1680     ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_VOFFS, crop->top);
1681     if (ret < 0)
1682         return ret;
1683
1684     ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_DVPHO, mode->width);
1685     if (ret < 0)
1686         return ret;
1687
1688     ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_DVPVO, mode->height);
1689     if (ret < 0)
1690         return ret;
1691
1692     ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_HTS, timings->htot);
1693     if (ret < 0)
1694         return ret;
1695
1696     ret = ov5640_write_reg16(sensor, OV5640_REG_TIMING_VTS,
1697                  mode->height + timings->vblank_def);
1698     if (ret < 0)
1699         return ret;
1700
1701     return 0;
1702 }
1703
1704 static void ov5640_load_regs(struct ov5640_dev *sensor,
1705                  const struct reg_value *regs, unsigned int regnum)
1706 {
1707     unsigned int i;
1708     u32 delay_ms;
1709     u16 reg_addr;
1710     u8 mask, val;
1711     int ret = 0;
1712
1713     for (i = 0; i < regnum; ++i, ++regs) {
1714         delay_ms = regs->delay_ms;
1715         reg_addr = regs->reg_addr;
1716         val = regs->val;
1717         mask = regs->mask;
1718
1719         /* remain in power down mode for DVP */
1720         if (regs->reg_addr == OV5640_REG_SYS_CTRL0 &&
1721             val == OV5640_REG_SYS_CTRL0_SW_PWUP &&
1722             !ov5640_is_csi2(sensor))
1723             continue;
1724
1725         if (mask)
1726             ret = ov5640_mod_reg(sensor, reg_addr, mask, val);
1727         else
1728             ret = ov5640_write_reg(sensor, reg_addr, val);
1729         if (ret)
1730             break;
1731
1732         if (delay_ms)
1733             usleep_range(1000 * delay_ms, 1000 * delay_ms + 100);
1734     }
1735 }
1736
1737 static int ov5640_set_autoexposure(struct ov5640_dev *sensor, bool on)
1738 {
1739     return ov5640_mod_reg(sensor, OV5640_REG_AEC_PK_MANUAL,
1740                   BIT(0), on ? 0 : BIT(0));
1741 }
1742
1743 /* read exposure, in number of line periods */
1744 static int ov5640_get_exposure(struct ov5640_dev *sensor)
1745 {
1746     int exp, ret;
1747     u8 temp;
1748
1749     ret = ov5640_read_reg(sensor, OV5640_REG_AEC_PK_EXPOSURE_HI, &temp);
1750     if (ret)
1751         return ret;
1752     exp = ((int)temp & 0x0f) << 16;
1753     ret = ov5640_read_reg(sensor, OV5640_REG_AEC_PK_EXPOSURE_MED, &temp);
1754     if (ret)
1755         return ret;
1756     exp |= ((int)temp << 8);
1757     ret = ov5640_read_reg(sensor, OV5640_REG_AEC_PK_EXPOSURE_LO, &temp);
1758     if (ret)
1759         return ret;
1760     exp |= (int)temp;
1761
1762     return exp >> 4;
1763 }
1764
1765 /* write exposure, given number of line periods */
1766 static int ov5640_set_exposure(struct ov5640_dev *sensor, u32 exposure)
1767 {
1768     int ret;
1769
1770     exposure <<= 4;
1771
1772     ret = ov5640_write_reg(sensor,
1773                    OV5640_REG_AEC_PK_EXPOSURE_LO,
1774                    exposure & 0xff);
1775     if (ret)
1776         return ret;
1777     ret = ov5640_write_reg(sensor,
1778                    OV5640_REG_AEC_PK_EXPOSURE_MED,
1779                    (exposure >> 8) & 0xff);
1780     if (ret)
1781         return ret;
1782     return ov5640_write_reg(sensor,
1783                 OV5640_REG_AEC_PK_EXPOSURE_HI,
1784                 (exposure >> 16) & 0x0f);
1785 }
1786
1787 static int ov5640_get_gain(struct ov5640_dev *sensor)
1788 {
1789     u16 gain;
1790     int ret;
1791
1792     ret = ov5640_read_reg16(sensor, OV5640_REG_AEC_PK_REAL_GAIN, &gain);
1793     if (ret)
1794         return ret;
1795
1796     return gain & 0x3ff;
1797 }
1798
1799 static int ov5640_set_gain(struct ov5640_dev *sensor, int gain)
1800 {
1801     return ov5640_write_reg16(sensor, OV5640_REG_AEC_PK_REAL_GAIN,
1802                   (u16)gain & 0x3ff);
1803 }
1804
1805 static int ov5640_set_autogain(struct ov5640_dev *sensor, bool on)
1806 {
1807     return ov5640_mod_reg(sensor, OV5640_REG_AEC_PK_MANUAL,
1808                   BIT(1), on ? 0 : BIT(1));
1809 }
1810
1811 static int ov5640_set_stream_dvp(struct ov5640_dev *sensor, bool on)
1812 {
1813     return ov5640_write_reg(sensor, OV5640_REG_SYS_CTRL0, on ?
1814                 OV5640_REG_SYS_CTRL0_SW_PWUP :
1815                 OV5640_REG_SYS_CTRL0_SW_PWDN);
1816 }
1817
1818 static int ov5640_set_stream_mipi(struct ov5640_dev *sensor, bool on)
1819 {
1820     int ret;
1821
1822     /*
1823      * Enable/disable the MIPI interface
1824      *
1825      * 0x300e = on ? 0x45 : 0x40
1826      *
1827      * FIXME: the sensor manual (version 2.03) reports
1828      * [7:5] = 000  : 1 data lane mode
1829      * [7:5] = 001  : 2 data lanes mode
1830      * But this settings do not work, while the following ones
1831      * have been validated for 2 data lanes mode.
1832      *
1833      * [7:5] = 010  : 2 data lanes mode
1834      * [4] = 0  : Power up MIPI HS Tx
1835      * [3] = 0  : Power up MIPI LS Rx
1836      * [2] = 1/0    : MIPI interface enable/disable
1837      * [1:0] = 01/00: FIXME: 'debug'
1838      */
1839     ret = ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00,
1840                    on ? 0x45 : 0x40);
1841     if (ret)
1842         return ret;
1843
1844     return ov5640_write_reg(sensor, OV5640_REG_FRAME_CTRL01,
1845                 on ? 0x00 : 0x0f);
1846 }
1847
1848 static int ov5640_get_sysclk(struct ov5640_dev *sensor)
1849 {
1850      /* calculate sysclk */
1851     u32 xvclk = sensor->xclk_freq / 10000;
1852     u32 multiplier, prediv, VCO, sysdiv, pll_rdiv;
1853     u32 sclk_rdiv_map[] = {1, 2, 4, 8};
1854     u32 bit_div2x = 1, sclk_rdiv, sysclk;
1855     u8 temp1, temp2;
1856     int ret;
1857
1858     ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL0, &temp1);
1859     if (ret)
1860         return ret;
1861     temp2 = temp1 & 0x0f;
1862     if (temp2 == 8 || temp2 == 10)
1863         bit_div2x = temp2 / 2;
1864
1865     ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL1, &temp1);
1866     if (ret)
1867         return ret;
1868     sysdiv = temp1 >> 4;
1869     if (sysdiv == 0)
1870         sysdiv = 16;
1871
1872     ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL2, &temp1);
1873     if (ret)
1874         return ret;
1875     multiplier = temp1;
1876
1877     ret = ov5640_read_reg(sensor, OV5640_REG_SC_PLL_CTRL3, &temp1);
1878     if (ret)
1879         return ret;
1880     prediv = temp1 & 0x0f;
1881     pll_rdiv = ((temp1 >> 4) & 0x01) + 1;
1882
1883     ret = ov5640_read_reg(sensor, OV5640_REG_SYS_ROOT_DIVIDER, &temp1);
1884     if (ret)
1885         return ret;
1886     temp2 = temp1 & 0x03;
1887     sclk_rdiv = sclk_rdiv_map[temp2];
1888
1889     if (!prediv || !sysdiv || !pll_rdiv || !bit_div2x)
1890         return -EINVAL;
1891
1892     VCO = xvclk * multiplier / prediv;
1893
1894     sysclk = VCO / sysdiv / pll_rdiv * 2 / bit_div2x / sclk_rdiv;
1895
1896     return sysclk;
1897 }
1898
1899 static int ov5640_set_night_mode(struct ov5640_dev *sensor)
1900 {
1901      /* read HTS from register settings */
1902     u8 mode;
1903     int ret;
1904
1905     ret = ov5640_read_reg(sensor, OV5640_REG_AEC_CTRL00, &mode);
1906     if (ret)
1907         return ret;
1908     mode &= 0xfb;
1909     return ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL00, mode);
1910 }
1911
1912 static int ov5640_get_hts(struct ov5640_dev *sensor)
1913 {
1914     /* read HTS from register settings */
1915     u16 hts;
1916     int ret;
1917
1918     ret = ov5640_read_reg16(sensor, OV5640_REG_TIMING_HTS, &hts);
1919     if (ret)
1920         return ret;
1921     return hts;
1922 }
1923
1924 static int ov5640_get_vts(struct ov5640_dev *sensor)
1925 {
1926     u16 vts;
1927     int ret;
1928
1929     ret = ov5640_read_reg16(sensor, OV5640_REG_TIMING_VTS, &vts);
1930     if (ret)
1931         return ret;
1932     return vts;
1933 }
1934
1935 static int ov5640_set_vts(struct ov5640_dev *sensor, int vts)
1936 {
1937     return ov5640_write_reg16(sensor, OV5640_REG_TIMING_VTS, vts);
1938 }
1939
1940 static int ov5640_get_light_freq(struct ov5640_dev *sensor)
1941 {
1942     /* get banding filter value */
1943     int ret, light_freq = 0;
1944     u8 temp, temp1;
1945
1946     ret = ov5640_read_reg(sensor, OV5640_REG_HZ5060_CTRL01, &temp);
1947     if (ret)
1948         return ret;
1949
1950     if (temp & 0x80) {
1951         /* manual */
1952         ret = ov5640_read_reg(sensor, OV5640_REG_HZ5060_CTRL00,
1953                       &temp1);
1954         if (ret)
1955             return ret;
1956         if (temp1 & 0x04) {
1957             /* 50Hz */
1958             light_freq = 50;
1959         } else {
1960             /* 60Hz */
1961             light_freq = 60;
1962         }
1963     } else {
1964         /* auto */
1965         ret = ov5640_read_reg(sensor, OV5640_REG_SIGMADELTA_CTRL0C,
1966                       &temp1);
1967         if (ret)
1968             return ret;
1969
1970         if (temp1 & 0x01) {
1971             /* 50Hz */
1972             light_freq = 50;
1973         } else {
1974             /* 60Hz */
1975         }
1976     }
1977
1978     return light_freq;
1979 }
1980
1981 static int ov5640_set_bandingfilter(struct ov5640_dev *sensor)
1982 {
1983     u32 band_step60, max_band60, band_step50, max_band50, prev_vts;
1984     int ret;
1985
1986     /* read preview PCLK */
1987     ret = ov5640_get_sysclk(sensor);
1988     if (ret < 0)
1989         return ret;
1990     if (ret == 0)
1991         return -EINVAL;
1992     sensor->prev_sysclk = ret;
1993     /* read preview HTS */
1994     ret = ov5640_get_hts(sensor);
1995     if (ret < 0)
1996         return ret;
1997     if (ret == 0)
1998         return -EINVAL;
1999     sensor->prev_hts = ret;
2000
2001     /* read preview VTS */
2002     ret = ov5640_get_vts(sensor);
2003     if (ret < 0)
2004         return ret;
2005     prev_vts = ret;
2006
2007     /* calculate banding filter */
2008     /* 60Hz */
2009     band_step60 = sensor->prev_sysclk * 100 / sensor->prev_hts * 100 / 120;
2010     ret = ov5640_write_reg16(sensor, OV5640_REG_AEC_B60_STEP, band_step60);
2011     if (ret)
2012         return ret;
2013     if (!band_step60)
2014         return -EINVAL;
2015     max_band60 = (int)((prev_vts - 4) / band_step60);
2016     ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL0D, max_band60);
2017     if (ret)
2018         return ret;
2019
2020     /* 50Hz */
2021     band_step50 = sensor->prev_sysclk * 100 / sensor->prev_hts;
2022     ret = ov5640_write_reg16(sensor, OV5640_REG_AEC_B50_STEP, band_step50);
2023     if (ret)
2024         return ret;
2025     if (!band_step50)
2026         return -EINVAL;
2027     max_band50 = (int)((prev_vts - 4) / band_step50);
2028     return ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL0E, max_band50);
2029 }
2030
2031 static int ov5640_set_ae_target(struct ov5640_dev *sensor, int target)
2032 {
2033     /* stable in high */
2034     u32 fast_high, fast_low;
2035     int ret;
2036
2037     sensor->ae_low = target * 23 / 25;  /* 0.92 */
2038     sensor->ae_high = target * 27 / 25; /* 1.08 */
2039
2040     fast_high = sensor->ae_high << 1;
2041     if (fast_high > 255)
2042         fast_high = 255;
2043
2044     fast_low = sensor->ae_low >> 1;
2045
2046     ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL0F, sensor->ae_high);
2047     if (ret)
2048         return ret;
2049     ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL10, sensor->ae_low);
2050     if (ret)
2051         return ret;
2052     ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL1B, sensor->ae_high);
2053     if (ret)
2054         return ret;
2055     ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL1E, sensor->ae_low);
2056     if (ret)
2057         return ret;
2058     ret = ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL11, fast_high);
2059     if (ret)
2060         return ret;
2061     return ov5640_write_reg(sensor, OV5640_REG_AEC_CTRL1F, fast_low);
2062 }
2063
2064 static int ov5640_get_binning(struct ov5640_dev *sensor)
2065 {
2066     u8 temp;
2067     int ret;
2068
2069     ret = ov5640_read_reg(sensor, OV5640_REG_TIMING_TC_REG21, &temp);
2070     if (ret)
2071         return ret;
2072
2073     return temp & BIT(0);
2074 }
2075
2076 static int ov5640_set_binning(struct ov5640_dev *sensor, bool enable)
2077 {
2078     int ret;
2079
2080     /*
2081      * TIMING TC REG21:
2082      * - [0]:   Horizontal binning enable
2083      */
2084     ret = ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG21,
2085                  BIT(0), enable ? BIT(0) : 0);
2086     if (ret)
2087         return ret;
2088     /*
2089      * TIMING TC REG20:
2090      * - [0]:   Undocumented, but hardcoded init sequences
2091      *      are always setting REG21/REG20 bit 0 to same value...
2092      */
2093     return ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG20,
2094                   BIT(0), enable ? BIT(0) : 0);
2095 }
2096
2097 static int ov5640_set_virtual_channel(struct ov5640_dev *sensor)
2098 {
2099     struct i2c_client *client = sensor->i2c_client;
2100     u8 temp, channel = virtual_channel;
2101     int ret;
2102
2103     if (channel > 3) {
2104         dev_err(&client->dev,
2105             "%s: wrong virtual_channel parameter, expected (0..3), got %d\n",
2106             __func__, channel);
2107         return -EINVAL;
2108     }
2109
2110     ret = ov5640_read_reg(sensor, OV5640_REG_DEBUG_MODE, &temp);
2111     if (ret)
2112         return ret;
2113     temp &= ~(3 << 6);
2114     temp |= (channel << 6);
2115     return ov5640_write_reg(sensor, OV5640_REG_DEBUG_MODE, temp);
2116 }
2117
2118 static const struct ov5640_mode_info *
2119 ov5640_find_mode(struct ov5640_dev *sensor, int width, int height, bool nearest)
2120 {
2121     const struct ov5640_mode_info *mode;
2122
2123     mode = v4l2_find_nearest_size(ov5640_mode_data,
2124                       ARRAY_SIZE(ov5640_mode_data),
2125                       width, height, width, height);
2126
2127     if (!mode ||
2128         (!nearest &&
2129          (mode->width != width || mode->height != height)))
2130         return NULL;
2131
2132     return mode;
2133 }
2134
2135 /*
2136  * sensor changes between scaling and subsampling, go through
2137  * exposure calculation
2138  */
2139 static int ov5640_set_mode_exposure_calc(struct ov5640_dev *sensor,
2140                      const struct ov5640_mode_info *mode)
2141 {
2142     u32 prev_shutter, prev_gain16;
2143     u32 cap_shutter, cap_gain16;
2144     u32 cap_sysclk, cap_hts, cap_vts;
2145     u32 light_freq, cap_bandfilt, cap_maxband;
2146     u32 cap_gain16_shutter;
2147     u8 average;
2148     int ret;
2149
2150     if (!mode->reg_data)
2151         return -EINVAL;
2152
2153     /* read preview shutter */
2154     ret = ov5640_get_exposure(sensor);
2155     if (ret < 0)
2156         return ret;
2157     prev_shutter = ret;
2158     ret = ov5640_get_binning(sensor);
2159     if (ret < 0)
2160         return ret;
2161     if (ret && mode->id != OV5640_MODE_720P_1280_720 &&
2162         mode->id != OV5640_MODE_1080P_1920_1080)
2163         prev_shutter *= 2;
2164
2165     /* read preview gain */
2166     ret = ov5640_get_gain(sensor);
2167     if (ret < 0)
2168         return ret;
2169     prev_gain16 = ret;
2170
2171     /* get average */
2172     ret = ov5640_read_reg(sensor, OV5640_REG_AVG_READOUT, &average);
2173     if (ret)
2174         return ret;
2175
2176     /* turn off night mode for capture */
2177     ret = ov5640_set_night_mode(sensor);
2178     if (ret < 0)
2179         return ret;
2180
2181     /* Write capture setting */
2182     ov5640_load_regs(sensor, mode->reg_data, mode->reg_data_size);
2183     ret = ov5640_set_timings(sensor, mode);
2184     if (ret < 0)
2185         return ret;
2186
2187     /* read capture VTS */
2188     ret = ov5640_get_vts(sensor);
2189     if (ret < 0)
2190         return ret;
2191     cap_vts = ret;
2192     ret = ov5640_get_hts(sensor);
2193     if (ret < 0)
2194         return ret;
2195     if (ret == 0)
2196         return -EINVAL;
2197     cap_hts = ret;
2198
2199     ret = ov5640_get_sysclk(sensor);
2200     if (ret < 0)
2201         return ret;
2202     if (ret == 0)
2203         return -EINVAL;
2204     cap_sysclk = ret;
2205
2206     /* calculate capture banding filter */
2207     ret = ov5640_get_light_freq(sensor);
2208     if (ret < 0)
2209         return ret;
2210     light_freq = ret;
2211
2212     if (light_freq == 60) {
2213         /* 60Hz */
2214         cap_bandfilt = cap_sysclk * 100 / cap_hts * 100 / 120;
2215     } else {
2216         /* 50Hz */
2217         cap_bandfilt = cap_sysclk * 100 / cap_hts;
2218     }
2219
2220     if (!sensor->prev_sysclk) {
2221         ret = ov5640_get_sysclk(sensor);
2222         if (ret < 0)
2223             return ret;
2224         if (ret == 0)
2225             return -EINVAL;
2226         sensor->prev_sysclk = ret;
2227     }
2228
2229     if (!cap_bandfilt)
2230         return -EINVAL;
2231
2232     cap_maxband = (int)((cap_vts - 4) / cap_bandfilt);
2233
2234     /* calculate capture shutter/gain16 */
2235     if (average > sensor->ae_low && average < sensor->ae_high) {
2236         /* in stable range */
2237         cap_gain16_shutter =
2238             prev_gain16 * prev_shutter *
2239             cap_sysclk / sensor->prev_sysclk *
2240             sensor->prev_hts / cap_hts *
2241             sensor->ae_target / average;
2242     } else {
2243         cap_gain16_shutter =
2244             prev_gain16 * prev_shutter *
2245             cap_sysclk / sensor->prev_sysclk *
2246             sensor->prev_hts / cap_hts;
2247     }
2248
2249     /* gain to shutter */
2250     if (cap_gain16_shutter < (cap_bandfilt * 16)) {
2251         /* shutter < 1/100 */
2252         cap_shutter = cap_gain16_shutter / 16;
2253         if (cap_shutter < 1)
2254             cap_shutter = 1;
2255
2256         cap_gain16 = cap_gain16_shutter / cap_shutter;
2257         if (cap_gain16 < 16)
2258             cap_gain16 = 16;
2259     } else {
2260         if (cap_gain16_shutter > (cap_bandfilt * cap_maxband * 16)) {
2261             /* exposure reach max */
2262             cap_shutter = cap_bandfilt * cap_maxband;
2263             if (!cap_shutter)
2264                 return -EINVAL;
2265
2266             cap_gain16 = cap_gain16_shutter / cap_shutter;
2267         } else {
2268             /* 1/100 < (cap_shutter = n/100) =< max */
2269             cap_shutter =
2270                 ((int)(cap_gain16_shutter / 16 / cap_bandfilt))
2271                 * cap_bandfilt;
2272             if (!cap_shutter)
2273                 return -EINVAL;
2274
2275             cap_gain16 = cap_gain16_shutter / cap_shutter;
2276         }
2277     }
2278
2279     /* set capture gain */
2280     ret = ov5640_set_gain(sensor, cap_gain16);
2281     if (ret)
2282         return ret;
2283
2284     /* write capture shutter */
2285     if (cap_shutter > (cap_vts - 4)) {
2286         cap_vts = cap_shutter + 4;
2287         ret = ov5640_set_vts(sensor, cap_vts);
2288         if (ret < 0)
2289             return ret;
2290     }
2291
2292     /* set exposure */
2293     return ov5640_set_exposure(sensor, cap_shutter);
2294 }
2295
2296 /*
2297  * if sensor changes inside scaling or subsampling
2298  * change mode directly
2299  */
2300 static int ov5640_set_mode_direct(struct ov5640_dev *sensor,
2301                   const struct ov5640_mode_info *mode)
2302 {
2303     if (!mode->reg_data)
2304         return -EINVAL;
2305
2306     /* Write capture setting */
2307     ov5640_load_regs(sensor, mode->reg_data, mode->reg_data_size);
2308     return ov5640_set_timings(sensor, mode);
2309 }
2310
2311 static int ov5640_set_mode(struct ov5640_dev *sensor)
2312 {
2313     const struct ov5640_mode_info *mode = sensor->current_mode;
2314     const struct ov5640_mode_info *orig_mode = sensor->last_mode;
2315     enum ov5640_downsize_mode dn_mode, orig_dn_mode;
2316     bool auto_gain = sensor->ctrls.auto_gain->val == 1;
2317     bool auto_exp =  sensor->ctrls.auto_exp->val == V4L2_EXPOSURE_AUTO;
2318     int ret;
2319
2320     dn_mode = mode->dn_mode;
2321     orig_dn_mode = orig_mode->dn_mode;
2322
2323     /* auto gain and exposure must be turned off when changing modes */
2324     if (auto_gain) {
2325         ret = ov5640_set_autogain(sensor, false);
2326         if (ret)
2327             return ret;
2328     }
2329
2330     if (auto_exp) {
2331         ret = ov5640_set_autoexposure(sensor, false);
2332         if (ret)
2333             goto restore_auto_gain;
2334     }
2335
2336     if (ov5640_is_csi2(sensor))
2337         ret = ov5640_set_mipi_pclk(sensor);
2338     else
2339         ret = ov5640_set_dvp_pclk(sensor);
2340     if (ret < 0)
2341         return 0;
2342
2343     if ((dn_mode == SUBSAMPLING && orig_dn_mode == SCALING) ||
2344         (dn_mode == SCALING && orig_dn_mode == SUBSAMPLING)) {
2345         /*
2346          * change between subsampling and scaling
2347          * go through exposure calculation
2348          */
2349         ret = ov5640_set_mode_exposure_calc(sensor, mode);
2350     } else {
2351         /*
2352          * change inside subsampling or scaling
2353          * download firmware directly
2354          */
2355         ret = ov5640_set_mode_direct(sensor, mode);
2356     }
2357     if (ret < 0)
2358         goto restore_auto_exp_gain;
2359
2360     /* restore auto gain and exposure */
2361     if (auto_gain)
2362         ov5640_set_autogain(sensor, true);
2363     if (auto_exp)
2364         ov5640_set_autoexposure(sensor, true);
2365
2366     ret = ov5640_set_binning(sensor, dn_mode != SCALING);
2367     if (ret < 0)
2368         return ret;
2369     ret = ov5640_set_ae_target(sensor, sensor->ae_target);
2370     if (ret < 0)
2371         return ret;
2372     ret = ov5640_get_light_freq(sensor);
2373     if (ret < 0)
2374         return ret;
2375     ret = ov5640_set_bandingfilter(sensor);
2376     if (ret < 0)
2377         return ret;
2378     ret = ov5640_set_virtual_channel(sensor);
2379     if (ret < 0)
2380         return ret;
2381
2382     sensor->pending_mode_change = false;
2383     sensor->last_mode = mode;
2384
2385     return 0;
2386
2387 restore_auto_exp_gain:
2388     if (auto_exp)
2389         ov5640_set_autoexposure(sensor, true);
2390 restore_auto_gain:
2391     if (auto_gain)
2392         ov5640_set_autogain(sensor, true);
2393
2394     return ret;
2395 }
2396
2397 static int ov5640_set_framefmt(struct ov5640_dev *sensor,
2398                    struct v4l2_mbus_framefmt *format);
2399
2400 /* restore the last set video mode after chip power-on */
2401 static int ov5640_restore_mode(struct ov5640_dev *sensor)
2402 {
2403     int ret;
2404
2405     /* first load the initial register values */
2406     ov5640_load_regs(sensor, ov5640_init_setting,
2407              ARRAY_SIZE(ov5640_init_setting));
2408
2409     ret = ov5640_mod_reg(sensor, OV5640_REG_SYS_ROOT_DIVIDER, 0x3f,
2410                  (ilog2(OV5640_SCLK2X_ROOT_DIV) << 2) |
2411                  ilog2(OV5640_SCLK_ROOT_DIV));
2412     if (ret)
2413         return ret;
2414
2415     /* now restore the last capture mode */
2416     ret = ov5640_set_mode(sensor);
2417     if (ret < 0)
2418         return ret;
2419
2420     return ov5640_set_framefmt(sensor, &sensor->fmt);
2421 }
2422
2423 static void ov5640_power(struct ov5640_dev *sensor, bool enable)
2424 {
2425     gpiod_set_value_cansleep(sensor->pwdn_gpio, enable ? 0 : 1);
2426 }
2427
2428 static void ov5640_reset(struct ov5640_dev *sensor)
2429 {
2430     if (!sensor->reset_gpio)
2431         return;
2432
2433     gpiod_set_value_cansleep(sensor->reset_gpio, 0);
2434
2435     /* camera power cycle */
2436     ov5640_power(sensor, false);
2437     usleep_range(5000, 10000);
2438     ov5640_power(sensor, true);
2439     usleep_range(5000, 10000);
2440
2441     gpiod_set_value_cansleep(sensor->reset_gpio, 1);
2442     usleep_range(1000, 2000);
2443
2444     gpiod_set_value_cansleep(sensor->reset_gpio, 0);
2445     usleep_range(20000, 25000);
2446 }
2447
2448 static int ov5640_set_power_on(struct ov5640_dev *sensor)
2449 {
2450     struct i2c_client *client = sensor->i2c_client;
2451     int ret;
2452
2453     ret = clk_prepare_enable(sensor->xclk);
2454     if (ret) {
2455         dev_err(&client->dev, "%s: failed to enable clock\n",
2456             __func__);
2457         return ret;
2458     }
2459
2460     ret = regulator_bulk_enable(OV5640_NUM_SUPPLIES,
2461                     sensor->supplies);
2462     if (ret) {
2463         dev_err(&client->dev, "%s: failed to enable regulators\n",
2464             __func__);
2465         goto xclk_off;
2466     }
2467
2468     ov5640_reset(sensor);
2469     ov5640_power(sensor, true);
2470
2471     ret = ov5640_init_slave_id(sensor);
2472     if (ret)
2473         goto power_off;
2474
2475     return 0;
2476
2477 power_off:
2478     ov5640_power(sensor, false);
2479     regulator_bulk_disable(OV5640_NUM_SUPPLIES, sensor->supplies);
2480 xclk_off:
2481     clk_disable_unprepare(sensor->xclk);
2482     return ret;
2483 }
2484
2485 static void ov5640_set_power_off(struct ov5640_dev *sensor)
2486 {
2487     ov5640_power(sensor, false);
2488     regulator_bulk_disable(OV5640_NUM_SUPPLIES, sensor->supplies);
2489     clk_disable_unprepare(sensor->xclk);
2490 }
2491
2492 static int ov5640_set_power_mipi(struct ov5640_dev *sensor, bool on)
2493 {
2494     int ret;
2495
2496     if (!on) {
2497         /* Reset MIPI bus settings to their default values. */
2498         ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x58);
2499         ov5640_write_reg(sensor, OV5640_REG_MIPI_CTRL00, 0x04);
2500         ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT00, 0x00);
2501         return 0;
2502     }
2503
2504     /*
2505      * Power up MIPI HS Tx and LS Rx; 2 data lanes mode
2506      *
2507      * 0x300e = 0x40
2508      * [7:5] = 010  : 2 data lanes mode (see FIXME note in
2509      *        "ov5640_set_stream_mipi()")
2510      * [4] = 0  : Power up MIPI HS Tx
2511      * [3] = 0  : Power up MIPI LS Rx
2512      * [2] = 0  : MIPI interface disabled
2513      */
2514     ret = ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x40);
2515     if (ret)
2516         return ret;
2517
2518     /*
2519      * Gate clock and set LP11 in 'no packets mode' (idle)
2520      *
2521      * 0x4800 = 0x24
2522      * [5] = 1  : Gate clock when 'no packets'
2523      * [2] = 1  : MIPI bus in LP11 when 'no packets'
2524      */
2525     ret = ov5640_write_reg(sensor, OV5640_REG_MIPI_CTRL00, 0x24);
2526     if (ret)
2527         return ret;
2528
2529     /*
2530      * Set data lanes and clock in LP11 when 'sleeping'
2531      *
2532      * 0x3019 = 0x70
2533      * [6] = 1  : MIPI data lane 2 in LP11 when 'sleeping'
2534      * [5] = 1  : MIPI data lane 1 in LP11 when 'sleeping'
2535      * [4] = 1  : MIPI clock lane in LP11 when 'sleeping'
2536      */
2537     ret = ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT00, 0x70);
2538     if (ret)
2539         return ret;
2540
2541     /* Give lanes some time to coax into LP11 state. */
2542     usleep_range(500, 1000);
2543
2544     return 0;
2545 }
2546
2547 static int ov5640_set_power_dvp(struct ov5640_dev *sensor, bool on)
2548 {
2549     unsigned int flags = sensor->ep.bus.parallel.flags;
2550     bool bt656 = sensor->ep.bus_type == V4L2_MBUS_BT656;
2551     u8 polarities = 0;
2552     int ret;
2553
2554     if (!on) {
2555         /* Reset settings to their default values. */
2556         ov5640_write_reg(sensor, OV5640_REG_CCIR656_CTRL00, 0x00);
2557         ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x58);
2558         ov5640_write_reg(sensor, OV5640_REG_POLARITY_CTRL00, 0x20);
2559         ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT_ENABLE01, 0x00);
2560         ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT_ENABLE02, 0x00);
2561         return 0;
2562     }
2563
2564     /*
2565      * Note about parallel port configuration.
2566      *
2567      * When configured in parallel mode, the OV5640 will
2568      * output 10 bits data on DVP data lines [9:0].
2569      * If only 8 bits data are wanted, the 8 bits data lines
2570      * of the camera interface must be physically connected
2571      * on the DVP data lines [9:2].
2572      *
2573      * Control lines polarity can be configured through
2574      * devicetree endpoint control lines properties.
2575      * If no endpoint control lines properties are set,
2576      * polarity will be as below:
2577      * - VSYNC: active high
2578      * - HREF:  active low
2579      * - PCLK:  active low
2580      *
2581      * VSYNC & HREF are not configured if BT656 bus mode is selected
2582      */
2583
2584     /*
2585      * BT656 embedded synchronization configuration
2586      *
2587      * CCIR656 CTRL00
2588      * - [7]:   SYNC code selection (0: auto generate sync code,
2589      *      1: sync code from regs 0x4732-0x4735)
2590      * - [6]:   f value in CCIR656 SYNC code when fixed f value
2591      * - [5]:   Fixed f value
2592      * - [4:3]: Blank toggle data options (00: data=1'h040/1'h200,
2593      *      01: data from regs 0x4736-0x4738, 10: always keep 0)
2594      * - [1]:   Clip data disable
2595      * - [0]:   CCIR656 mode enable
2596      *
2597      * Default CCIR656 SAV/EAV mode with default codes
2598      * SAV=0xff000080 & EAV=0xff00009d is enabled here with settings:
2599      * - CCIR656 mode enable
2600      * - auto generation of sync codes
2601      * - blank toggle data 1'h040/1'h200
2602      * - clip reserved data (0x00 & 0xff changed to 0x01 & 0xfe)
2603      */
2604     ret = ov5640_write_reg(sensor, OV5640_REG_CCIR656_CTRL00,
2605                    bt656 ? 0x01 : 0x00);
2606     if (ret)
2607         return ret;
2608
2609     /*
2610      * configure parallel port control lines polarity
2611      *
2612      * POLARITY CTRL0
2613      * - [5]:   PCLK polarity (0: active low, 1: active high)
2614      * - [1]:   HREF polarity (0: active low, 1: active high)
2615      * - [0]:   VSYNC polarity (mismatch here between
2616      *      datasheet and hardware, 0 is active high
2617      *      and 1 is active low...)
2618      */
2619     if (!bt656) {
2620         if (flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
2621             polarities |= BIT(1);
2622         if (flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
2623             polarities |= BIT(0);
2624     }
2625     if (flags & V4L2_MBUS_PCLK_SAMPLE_RISING)
2626         polarities |= BIT(5);
2627
2628     ret = ov5640_write_reg(sensor, OV5640_REG_POLARITY_CTRL00, polarities);
2629     if (ret)
2630         return ret;
2631
2632     /*
2633      * powerdown MIPI TX/RX PHY & enable DVP
2634      *
2635      * MIPI CONTROL 00
2636      * [4] = 1  : Power down MIPI HS Tx
2637      * [3] = 1  : Power down MIPI LS Rx
2638      * [2] = 0  : DVP enable (MIPI disable)
2639      */
2640     ret = ov5640_write_reg(sensor, OV5640_REG_IO_MIPI_CTRL00, 0x18);
2641     if (ret)
2642         return ret;
2643
2644     /*
2645      * enable VSYNC/HREF/PCLK DVP control lines
2646      * & D[9:6] DVP data lines
2647      *
2648      * PAD OUTPUT ENABLE 01
2649      * - 6:     VSYNC output enable
2650      * - 5:     HREF output enable
2651      * - 4:     PCLK output enable
2652      * - [3:0]: D[9:6] output enable
2653      */
2654     ret = ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT_ENABLE01,
2655                    bt656 ? 0x1f : 0x7f);
2656     if (ret)
2657         return ret;
2658
2659     /*
2660      * enable D[5:0] DVP data lines
2661      *
2662      * PAD OUTPUT ENABLE 02
2663      * - [7:2]: D[5:0] output enable
2664      */
2665     return ov5640_write_reg(sensor, OV5640_REG_PAD_OUTPUT_ENABLE02, 0xfc);
2666 }
2667
2668 static int ov5640_set_power(struct ov5640_dev *sensor, bool on)
2669 {
2670     int ret = 0;
2671
2672     if (on) {
2673         ret = ov5640_set_power_on(sensor);
2674         if (ret)
2675             return ret;
2676
2677         ret = ov5640_restore_mode(sensor);
2678         if (ret)
2679             goto power_off;
2680     }
2681
2682     if (sensor->ep.bus_type == V4L2_MBUS_CSI2_DPHY)
2683         ret = ov5640_set_power_mipi(sensor, on);
2684     else
2685         ret = ov5640_set_power_dvp(sensor, on);
2686     if (ret)
2687         goto power_off;
2688
2689     if (!on)
2690         ov5640_set_power_off(sensor);
2691
2692     return 0;
2693
2694 power_off:
2695     ov5640_set_power_off(sensor);
2696     return ret;
2697 }
2698
2699 /* --------------- Subdev Operations --------------- */
2700
2701 static int ov5640_s_power(struct v4l2_subdev *sd, int on)
2702 {
2703     struct ov5640_dev *sensor = to_ov5640_dev(sd);
2704     int ret = 0;
2705
2706     mutex_lock(&sensor->lock);
2707
2708     /*
2709      * If the power count is modified from 0 to != 0 or from != 0 to 0,
2710      * update the power state.
2711      */
2712     if (sensor->power_count == !on) {
2713         ret = ov5640_set_power(sensor, !!on);
2714         if (ret)
2715             goto out;
2716     }
2717
2718     /* Update the power count. */
2719     sensor->power_count += on ? 1 : -1;
2720     WARN_ON(sensor->power_count < 0);
2721 out:
2722     mutex_unlock(&sensor->lock);
2723
2724     if (on && !ret && sensor->power_count == 1) {
2725         /* restore controls */
2726         ret = v4l2_ctrl_handler_setup(&sensor->ctrls.handler);
2727     }
2728
2729     return ret;
2730 }
2731
2732 static int ov5640_try_frame_interval(struct ov5640_dev *sensor,
2733                      struct v4l2_fract *fi,
2734                      u32 width, u32 height)
2735 {
2736     const struct ov5640_mode_info *mode;
2737     enum ov5640_frame_rate rate = OV5640_15_FPS;
2738     int minfps, maxfps, best_fps, fps;
2739     int i;
2740
2741     minfps = ov5640_framerates[OV5640_15_FPS];
2742     maxfps = ov5640_framerates[OV5640_60_FPS];
2743
2744     if (fi->numerator == 0) {
2745         fi->denominator = maxfps;
2746         fi->numerator = 1;
2747         rate = OV5640_60_FPS;
2748         goto find_mode;
2749     }
2750
2751     fps = clamp_val(DIV_ROUND_CLOSEST(fi->denominator, fi->numerator),
2752             minfps, maxfps);
2753
2754     best_fps = minfps;
2755     for (i = 0; i < ARRAY_SIZE(ov5640_framerates); i++) {
2756         int curr_fps = ov5640_framerates[i];
2757
2758         if (abs(curr_fps - fps) < abs(best_fps - fps)) {
2759             best_fps = curr_fps;
2760             rate = i;
2761         }
2762     }
2763
2764     fi->numerator = 1;
2765     fi->denominator = best_fps;
2766
2767 find_mode:
2768     mode = ov5640_find_mode(sensor, width, height, false);
2769     return mode ? rate : -EINVAL;
2770 }
2771
2772 static int ov5640_get_fmt(struct v4l2_subdev *sd,
2773               struct v4l2_subdev_state *sd_state,
2774               struct v4l2_subdev_format *format)
2775 {
2776     struct ov5640_dev *sensor = to_ov5640_dev(sd);
2777     struct v4l2_mbus_framefmt *fmt;
2778
2779     if (format->pad != 0)
2780         return -EINVAL;
2781
2782     mutex_lock(&sensor->lock);
2783
2784     if (format->which == V4L2_SUBDEV_FORMAT_TRY)
2785         fmt = v4l2_subdev_get_try_format(&sensor->sd, sd_state,
2786                          format->pad);
2787     else
2788         fmt = &sensor->fmt;
2789
2790     format->format = *fmt;
2791
2792     mutex_unlock(&sensor->lock);
2793
2794     return 0;
2795 }
2796
2797 static int ov5640_try_fmt_internal(struct v4l2_subdev *sd,
2798                    struct v4l2_mbus_framefmt *fmt,
2799                    enum ov5640_frame_rate fr,
2800                    const struct ov5640_mode_info **new_mode)
2801 {
2802     struct ov5640_dev *sensor = to_ov5640_dev(sd);
2803     const struct ov5640_mode_info *mode;
2804     const struct ov5640_pixfmt *pixfmt;
2805     unsigned int bpp;
2806
2807     mode = ov5640_find_mode(sensor, fmt->width, fmt->height, true);
2808     if (!mode)
2809         return -EINVAL;
2810
2811     pixfmt = ov5640_code_to_pixfmt(sensor, fmt->code);
2812     bpp = pixfmt->bpp;
2813
2814     /*
2815      * Adjust mode according to bpp:
2816      * - 8bpp modes work for resolution >= 1280x720
2817      * - 24bpp modes work resolution < 1280x720
2818      */
2819     if (bpp == 8 && mode->width < 1280)
2820         mode = &ov5640_mode_data[OV5640_MODE_720P_1280_720];
2821     else if (bpp == 24 && mode->width > 1024)
2822         mode = &ov5640_mode_data[OV5640_MODE_XGA_1024_768];
2823
2824     fmt->width = mode->width;
2825     fmt->height = mode->height;
2826
2827     if (new_mode)
2828         *new_mode = mode;
2829
2830     fmt->code = pixfmt->code;
2831     fmt->colorspace = pixfmt->colorspace;
2832     fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
2833     fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
2834     fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
2835
2836     return 0;
2837 }
2838
2839 static int ov5640_update_pixel_rate(struct ov5640_dev *sensor)
2840 {
2841     const struct ov5640_mode_info *mode = sensor->current_mode;
2842     enum ov5640_pixel_rate_id pixel_rate_id = mode->pixel_rate;
2843     struct v4l2_mbus_framefmt *fmt = &sensor->fmt;
2844     const struct ov5640_timings *timings;
2845     s32 exposure_val, exposure_max;
2846     unsigned int hblank;
2847     unsigned int i = 0;
2848     u32 pixel_rate;
2849     s64 link_freq;
2850     u32 num_lanes;
2851     u32 vblank;
2852     u32 bpp;
2853
2854     /*
2855      * Update the pixel rate control value.
2856      *
2857      * For DVP mode, maintain the pixel rate calculation using fixed FPS.
2858      */
2859     if (!ov5640_is_csi2(sensor)) {
2860         __v4l2_ctrl_s_ctrl_int64(sensor->ctrls.pixel_rate,
2861                      ov5640_calc_pixel_rate(sensor));
2862
2863         return 0;
2864     }
2865
2866     /*
2867      * The MIPI CSI-2 link frequency should comply with the CSI-2
2868      * specification and be lower than 1GHz.
2869      *
2870      * Start from the suggested pixel_rate for the current mode and
2871      * progressively slow it down if it exceeds 1GHz.
2872      */
2873     num_lanes = sensor->ep.bus.mipi_csi2.num_data_lanes;
2874     bpp = ov5640_code_to_bpp(sensor, fmt->code);
2875     do {
2876         pixel_rate = ov5640_pixel_rates[pixel_rate_id];
2877         link_freq = pixel_rate * bpp / (2 * num_lanes);
2878     } while (link_freq >= 1000000000U &&
2879          ++pixel_rate_id < OV5640_NUM_PIXEL_RATES);
2880
2881     sensor->current_link_freq = link_freq;
2882
2883     /*
2884      * Higher link rates require the clock tree to be programmed with
2885      * 'mipi_div' = 1; this has the effect of halving the actual output
2886      * pixel rate in the MIPI domain.
2887      *
2888      * Adjust the pixel rate and link frequency control value to report it
2889      * correctly to userspace.
2890      */
2891     if (link_freq > OV5640_LINK_RATE_MAX) {
2892         pixel_rate /= 2;
2893         link_freq /= 2;
2894     }
2895
2896     for (i = 0; i < ARRAY_SIZE(ov5640_csi2_link_freqs); ++i) {
2897         if (ov5640_csi2_link_freqs[i] == link_freq)
2898             break;
2899     }
2900     WARN_ON(i == ARRAY_SIZE(ov5640_csi2_link_freqs));
2901
2902     __v4l2_ctrl_s_ctrl_int64(sensor->ctrls.pixel_rate, pixel_rate);
2903     __v4l2_ctrl_s_ctrl(sensor->ctrls.link_freq, i);
2904
2905     timings = ov5640_timings(sensor, mode);
2906     hblank = timings->htot - mode->width;
2907     __v4l2_ctrl_modify_range(sensor->ctrls.hblank,
2908                  hblank, hblank, 1, hblank);
2909
2910     vblank = timings->vblank_def;
2911
2912     if (sensor->current_fr != mode->def_fps) {
2913         /*
2914          * Compute the vertical blanking according to the framerate
2915          * configured with s_frame_interval.
2916          */
2917         int fie_num = sensor->frame_interval.numerator;
2918         int fie_denom = sensor->frame_interval.denominator;
2919
2920         vblank = ((fie_num * pixel_rate / fie_denom) / timings->htot) -
2921             mode->height;
2922     }
2923
2924     __v4l2_ctrl_modify_range(sensor->ctrls.vblank, OV5640_MIN_VBLANK,
2925                  OV5640_MAX_VTS - mode->height, 1, vblank);
2926     __v4l2_ctrl_s_ctrl(sensor->ctrls.vblank, vblank);
2927
2928     exposure_max = timings->crop.height + vblank - 4;
2929     exposure_val = clamp_t(s32, sensor->ctrls.exposure->val,
2930                    sensor->ctrls.exposure->minimum,
2931                    exposure_max);
2932
2933     __v4l2_ctrl_modify_range(sensor->ctrls.exposure,
2934                  sensor->ctrls.exposure->minimum,
2935                  exposure_max, 1, exposure_val);
2936
2937     return 0;
2938 }
2939
2940 static int ov5640_set_fmt(struct v4l2_subdev *sd,
2941               struct v4l2_subdev_state *sd_state,
2942               struct v4l2_subdev_format *format)
2943 {
2944     struct ov5640_dev *sensor = to_ov5640_dev(sd);
2945     const struct ov5640_mode_info *new_mode;
2946     struct v4l2_mbus_framefmt *mbus_fmt = &format->format;
2947     int ret;
2948
2949     if (format->pad != 0)
2950         return -EINVAL;
2951
2952     mutex_lock(&sensor->lock);
2953
2954     if (sensor->streaming) {
2955         ret = -EBUSY;
2956         goto out;
2957     }
2958
2959     ret = ov5640_try_fmt_internal(sd, mbus_fmt,
2960                       sensor->current_fr, &new_mode);
2961     if (ret)
2962         goto out;
2963
2964     if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
2965         *v4l2_subdev_get_try_format(sd, sd_state, 0) = *mbus_fmt;
2966         goto out;
2967     }
2968
2969     if (new_mode != sensor->current_mode) {
2970         sensor->current_fr = new_mode->def_fps;
2971         sensor->current_mode = new_mode;
2972         sensor->pending_mode_change = true;
2973     }
2974     if (mbus_fmt->code != sensor->fmt.code)
2975         sensor->pending_fmt_change = true;
2976
2977     /* update format even if code is unchanged, resolution might change */
2978     sensor->fmt = *mbus_fmt;
2979
2980     ov5640_update_pixel_rate(sensor);
2981
2982 out:
2983     mutex_unlock(&sensor->lock);
2984     return ret;
2985 }
2986
2987 static int ov5640_get_selection(struct v4l2_subdev *sd,
2988                 struct v4l2_subdev_state *sd_state,
2989                 struct v4l2_subdev_selection *sel)
2990 {
2991     struct ov5640_dev *sensor = to_ov5640_dev(sd);
2992     const struct ov5640_mode_info *mode = sensor->current_mode;
2993     const struct ov5640_timings *timings;
2994
2995     switch (sel->target) {
2996     case V4L2_SEL_TGT_CROP: {
2997         mutex_lock(&sensor->lock);
2998         timings = ov5640_timings(sensor, mode);
2999         sel->r = timings->analog_crop;
3000         mutex_unlock(&sensor->lock);
3001
3002         return 0;
3003     }
3004
3005     case V4L2_SEL_TGT_NATIVE_SIZE:
3006     case V4L2_SEL_TGT_CROP_BOUNDS:
3007         sel->r.top = 0;
3008         sel->r.left = 0;
3009         sel->r.width = OV5640_NATIVE_WIDTH;
3010         sel->r.height = OV5640_NATIVE_HEIGHT;
3011
3012         return 0;
3013
3014     case V4L2_SEL_TGT_CROP_DEFAULT:
3015         sel->r.top = OV5640_PIXEL_ARRAY_TOP;
3016         sel->r.left = OV5640_PIXEL_ARRAY_LEFT;
3017         sel->r.width = OV5640_PIXEL_ARRAY_WIDTH;
3018         sel->r.height = OV5640_PIXEL_ARRAY_HEIGHT;
3019
3020         return 0;
3021     }
3022
3023     return -EINVAL;
3024 }
3025
3026 static int ov5640_set_framefmt(struct ov5640_dev *sensor,
3027                    struct v4l2_mbus_framefmt *format)
3028 {
3029     bool is_jpeg = format->code == MEDIA_BUS_FMT_JPEG_1X8;
3030     const struct ov5640_pixfmt *pixfmt;
3031     int ret = 0;
3032
3033     pixfmt = ov5640_code_to_pixfmt(sensor, format->code);
3034
3035     /* FORMAT CONTROL00: YUV and RGB formatting */
3036     ret = ov5640_write_reg(sensor, OV5640_REG_FORMAT_CONTROL00,
3037                    pixfmt->ctrl00);
3038     if (ret)
3039         return ret;
3040
3041     /* FORMAT MUX CONTROL: ISP YUV or RGB */
3042     ret = ov5640_write_reg(sensor, OV5640_REG_ISP_FORMAT_MUX_CTRL,
3043                    pixfmt->mux);
3044     if (ret)
3045         return ret;
3046
3047     /*
3048      * TIMING TC REG21:
3049      * - [5]:   JPEG enable
3050      */
3051     ret = ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG21,
3052                  BIT(5), is_jpeg ? BIT(5) : 0);
3053     if (ret)
3054         return ret;
3055
3056     /*
3057      * SYSTEM RESET02:
3058      * - [4]:   Reset JFIFO
3059      * - [3]:   Reset SFIFO
3060      * - [2]:   Reset JPEG
3061      */
3062     ret = ov5640_mod_reg(sensor, OV5640_REG_SYS_RESET02,
3063                  BIT(4) | BIT(3) | BIT(2),
3064                  is_jpeg ? 0 : (BIT(4) | BIT(3) | BIT(2)));
3065     if (ret)
3066         return ret;
3067
3068     /*
3069      * CLOCK ENABLE02:
3070      * - [5]:   Enable JPEG 2x clock
3071      * - [3]:   Enable JPEG clock
3072      */
3073     return ov5640_mod_reg(sensor, OV5640_REG_SYS_CLOCK_ENABLE02,
3074                   BIT(5) | BIT(3),
3075                   is_jpeg ? (BIT(5) | BIT(3)) : 0);
3076 }
3077
3078 /*
3079  * Sensor Controls.
3080  */
3081
3082 static int ov5640_set_ctrl_hue(struct ov5640_dev *sensor, int value)
3083 {
3084     int ret;
3085
3086     if (value) {
3087         ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0,
3088                      BIT(0), BIT(0));
3089         if (ret)
3090             return ret;
3091         ret = ov5640_write_reg16(sensor, OV5640_REG_SDE_CTRL1, value);
3092     } else {
3093         ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0, BIT(0), 0);
3094     }
3095
3096     return ret;
3097 }
3098
3099 static int ov5640_set_ctrl_contrast(struct ov5640_dev *sensor, int value)
3100 {
3101     int ret;
3102
3103     if (value) {
3104         ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0,
3105                      BIT(2), BIT(2));
3106         if (ret)
3107             return ret;
3108         ret = ov5640_write_reg(sensor, OV5640_REG_SDE_CTRL5,
3109                        value & 0xff);
3110     } else {
3111         ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0, BIT(2), 0);
3112     }
3113
3114     return ret;
3115 }
3116
3117 static int ov5640_set_ctrl_saturation(struct ov5640_dev *sensor, int value)
3118 {
3119     int ret;
3120
3121     if (value) {
3122         ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0,
3123                      BIT(1), BIT(1));
3124         if (ret)
3125             return ret;
3126         ret = ov5640_write_reg(sensor, OV5640_REG_SDE_CTRL3,
3127                        value & 0xff);
3128         if (ret)
3129             return ret;
3130         ret = ov5640_write_reg(sensor, OV5640_REG_SDE_CTRL4,
3131                        value & 0xff);
3132     } else {
3133         ret = ov5640_mod_reg(sensor, OV5640_REG_SDE_CTRL0, BIT(1), 0);
3134     }
3135
3136     return ret;
3137 }
3138
3139 static int ov5640_set_ctrl_white_balance(struct ov5640_dev *sensor, int awb)
3140 {
3141     int ret;
3142
3143     ret = ov5640_mod_reg(sensor, OV5640_REG_AWB_MANUAL_CTRL,
3144                  BIT(0), awb ? 0 : 1);
3145     if (ret)
3146         return ret;
3147
3148     if (!awb) {
3149         u16 red = (u16)sensor->ctrls.red_balance->val;
3150         u16 blue = (u16)sensor->ctrls.blue_balance->val;
3151
3152         ret = ov5640_write_reg16(sensor, OV5640_REG_AWB_R_GAIN, red);
3153         if (ret)
3154             return ret;
3155         ret = ov5640_write_reg16(sensor, OV5640_REG_AWB_B_GAIN, blue);
3156     }
3157
3158     return ret;
3159 }
3160
3161 static int ov5640_set_ctrl_exposure(struct ov5640_dev *sensor,
3162                     enum v4l2_exposure_auto_type auto_exposure)
3163 {
3164     struct ov5640_ctrls *ctrls = &sensor->ctrls;
3165     bool auto_exp = (auto_exposure == V4L2_EXPOSURE_AUTO);
3166     int ret = 0;
3167
3168     if (ctrls->auto_exp->is_new) {
3169         ret = ov5640_set_autoexposure(sensor, auto_exp);
3170         if (ret)
3171             return ret;
3172     }
3173
3174     if (!auto_exp && ctrls->exposure->is_new) {
3175         u16 max_exp;
3176
3177         ret = ov5640_read_reg16(sensor, OV5640_REG_AEC_PK_VTS,
3178                     &max_exp);
3179         if (ret)
3180             return ret;
3181         ret = ov5640_get_vts(sensor);
3182         if (ret < 0)
3183             return ret;
3184         max_exp += ret;
3185         ret = 0;
3186
3187         if (ctrls->exposure->val < max_exp)
3188             ret = ov5640_set_exposure(sensor, ctrls->exposure->val);
3189     }
3190
3191     return ret;
3192 }
3193
3194 static int ov5640_set_ctrl_gain(struct ov5640_dev *sensor, bool auto_gain)
3195 {
3196     struct ov5640_ctrls *ctrls = &sensor->ctrls;
3197     int ret = 0;
3198
3199     if (ctrls->auto_gain->is_new) {
3200         ret = ov5640_set_autogain(sensor, auto_gain);
3201         if (ret)
3202             return ret;
3203     }
3204
3205     if (!auto_gain && ctrls->gain->is_new)
3206         ret = ov5640_set_gain(sensor, ctrls->gain->val);
3207
3208     return ret;
3209 }
3210
3211 static const char * const test_pattern_menu[] = {
3212     "Disabled",
3213     "Color bars",
3214     "Color bars w/ rolling bar",
3215     "Color squares",
3216     "Color squares w/ rolling bar",
3217 };
3218
3219 #define OV5640_TEST_ENABLE      BIT(7)
3220 #define OV5640_TEST_ROLLING     BIT(6)  /* rolling horizontal bar */
3221 #define OV5640_TEST_TRANSPARENT     BIT(5)
3222 #define OV5640_TEST_SQUARE_BW       BIT(4)  /* black & white squares */
3223 #define OV5640_TEST_BAR_STANDARD    (0 << 2)
3224 #define OV5640_TEST_BAR_VERT_CHANGE_1   (1 << 2)
3225 #define OV5640_TEST_BAR_HOR_CHANGE  (2 << 2)
3226 #define OV5640_TEST_BAR_VERT_CHANGE_2   (3 << 2)
3227 #define OV5640_TEST_BAR         (0 << 0)
3228 #define OV5640_TEST_RANDOM      (1 << 0)
3229 #define OV5640_TEST_SQUARE      (2 << 0)
3230 #define OV5640_TEST_BLACK       (3 << 0)
3231
3232 static const u8 test_pattern_val[] = {
3233     0,
3234     OV5640_TEST_ENABLE | OV5640_TEST_BAR_VERT_CHANGE_1 |
3235         OV5640_TEST_BAR,
3236     OV5640_TEST_ENABLE | OV5640_TEST_ROLLING |
3237         OV5640_TEST_BAR_VERT_CHANGE_1 | OV5640_TEST_BAR,
3238     OV5640_TEST_ENABLE | OV5640_TEST_SQUARE,
3239     OV5640_TEST_ENABLE | OV5640_TEST_ROLLING | OV5640_TEST_SQUARE,
3240 };
3241
3242 static int ov5640_set_ctrl_test_pattern(struct ov5640_dev *sensor, int value)
3243 {
3244     return ov5640_write_reg(sensor, OV5640_REG_PRE_ISP_TEST_SET1,
3245                 test_pattern_val[value]);
3246 }
3247
3248 static int ov5640_set_ctrl_light_freq(struct ov5640_dev *sensor, int value)
3249 {
3250     int ret;
3251
3252     ret = ov5640_mod_reg(sensor, OV5640_REG_HZ5060_CTRL01, BIT(7),
3253                  (value == V4L2_CID_POWER_LINE_FREQUENCY_AUTO) ?
3254                  0 : BIT(7));
3255     if (ret)
3256         return ret;
3257
3258     return ov5640_mod_reg(sensor, OV5640_REG_HZ5060_CTRL00, BIT(2),
3259                   (value == V4L2_CID_POWER_LINE_FREQUENCY_50HZ) ?
3260                   BIT(2) : 0);
3261 }
3262
3263 static int ov5640_set_ctrl_hflip(struct ov5640_dev *sensor, int value)
3264 {
3265     /*
3266      * If sensor is mounted upside down, mirror logic is inversed.
3267      *
3268      * Sensor is a BSI (Back Side Illuminated) one,
3269      * so image captured is physically mirrored.
3270      * This is why mirror logic is inversed in
3271      * order to cancel this mirror effect.
3272      */
3273
3274     /*
3275      * TIMING TC REG21:
3276      * - [2]:   ISP mirror
3277      * - [1]:   Sensor mirror
3278      */
3279     return ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG21,
3280                   BIT(2) | BIT(1),
3281                   (!(value ^ sensor->upside_down)) ?
3282                   (BIT(2) | BIT(1)) : 0);
3283 }
3284
3285 static int ov5640_set_ctrl_vflip(struct ov5640_dev *sensor, int value)
3286 {
3287     /* If sensor is mounted upside down, flip logic is inversed */
3288
3289     /*
3290      * TIMING TC REG20:
3291      * - [2]:   ISP vflip
3292      * - [1]:   Sensor vflip
3293      */
3294     return ov5640_mod_reg(sensor, OV5640_REG_TIMING_TC_REG20,
3295                   BIT(2) | BIT(1),
3296                   (value ^ sensor->upside_down) ?
3297                   (BIT(2) | BIT(1)) : 0);
3298 }
3299
3300 static int ov5640_set_ctrl_vblank(struct ov5640_dev *sensor, int value)
3301 {
3302     const struct ov5640_mode_info *mode = sensor->current_mode;
3303
3304     /* Update the VTOT timing register value. */
3305     return ov5640_write_reg16(sensor, OV5640_REG_TIMING_VTS,
3306                   mode->height + value);
3307 }
3308
3309 static int ov5640_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
3310 {
3311     struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
3312     struct ov5640_dev *sensor = to_ov5640_dev(sd);
3313     int val;
3314
3315     /* v4l2_ctrl_lock() locks our own mutex */
3316
3317     switch (ctrl->id) {
3318     case V4L2_CID_AUTOGAIN:
3319         val = ov5640_get_gain(sensor);
3320         if (val < 0)
3321             return val;
3322         sensor->ctrls.gain->val = val;
3323         break;
3324     case V4L2_CID_EXPOSURE_AUTO:
3325         val = ov5640_get_exposure(sensor);
3326         if (val < 0)
3327             return val;
3328         sensor->ctrls.exposure->val = val;
3329         break;
3330     }
3331
3332     return 0;
3333 }
3334
3335 static int ov5640_s_ctrl(struct v4l2_ctrl *ctrl)
3336 {
3337     struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
3338     struct ov5640_dev *sensor = to_ov5640_dev(sd);
3339     const struct ov5640_mode_info *mode = sensor->current_mode;
3340     const struct ov5640_timings *timings;
3341     unsigned int exp_max;
3342     int ret;
3343
3344     /* v4l2_ctrl_lock() locks our own mutex */
3345
3346     switch (ctrl->id) {
3347     case V4L2_CID_VBLANK:
3348         /* Update the exposure range to the newly programmed vblank. */
3349         timings = ov5640_timings(sensor, mode);
3350         exp_max = mode->height + ctrl->val - 4;
3351         __v4l2_ctrl_modify_range(sensor->ctrls.exposure,
3352                      sensor->ctrls.exposure->minimum,
3353                      exp_max, sensor->ctrls.exposure->step,
3354                      timings->vblank_def);
3355         break;
3356     }
3357
3358     /*
3359      * If the device is not powered up by the host driver do
3360      * not apply any controls to H/W at this time. Instead
3361      * the controls will be restored right after power-up.
3362      */
3363     if (sensor->power_count == 0)
3364         return 0;
3365
3366     switch (ctrl->id) {
3367     case V4L2_CID_AUTOGAIN:
3368         ret = ov5640_set_ctrl_gain(sensor, ctrl->val);
3369         break;
3370     case V4L2_CID_EXPOSURE_AUTO:
3371         ret = ov5640_set_ctrl_exposure(sensor, ctrl->val);
3372         break;
3373     case V4L2_CID_AUTO_WHITE_BALANCE:
3374         ret = ov5640_set_ctrl_white_balance(sensor, ctrl->val);
3375         break;
3376     case V4L2_CID_HUE:
3377         ret = ov5640_set_ctrl_hue(sensor, ctrl->val);
3378         break;
3379     case V4L2_CID_CONTRAST:
3380         ret = ov5640_set_ctrl_contrast(sensor, ctrl->val);
3381         break;
3382     case V4L2_CID_SATURATION:
3383         ret = ov5640_set_ctrl_saturation(sensor, ctrl->val);
3384         break;
3385     case V4L2_CID_TEST_PATTERN:
3386         ret = ov5640_set_ctrl_test_pattern(sensor, ctrl->val);
3387         break;
3388     case V4L2_CID_POWER_LINE_FREQUENCY:
3389         ret = ov5640_set_ctrl_light_freq(sensor, ctrl->val);
3390         break;
3391     case V4L2_CID_HFLIP:
3392         ret = ov5640_set_ctrl_hflip(sensor, ctrl->val);
3393         break;
3394     case V4L2_CID_VFLIP:
3395         ret = ov5640_set_ctrl_vflip(sensor, ctrl->val);
3396         break;
3397     case V4L2_CID_VBLANK:
3398         ret = ov5640_set_ctrl_vblank(sensor, ctrl->val);
3399         break;
3400     default:
3401         ret = -EINVAL;
3402         break;
3403     }
3404
3405     return ret;
3406 }
3407
3408 static const struct v4l2_ctrl_ops ov5640_ctrl_ops = {
3409     .g_volatile_ctrl = ov5640_g_volatile_ctrl,
3410     .s_ctrl = ov5640_s_ctrl,
3411 };
3412
3413 static int ov5640_init_controls(struct ov5640_dev *sensor)
3414 {
3415     const struct ov5640_mode_info *mode = sensor->current_mode;
3416     const struct v4l2_ctrl_ops *ops = &ov5640_ctrl_ops;
3417     struct ov5640_ctrls *ctrls = &sensor->ctrls;
3418     struct v4l2_ctrl_handler *hdl = &ctrls->handler;
3419     struct v4l2_fwnode_device_properties props;
3420     const struct ov5640_timings *timings;
3421     unsigned int max_vblank;
3422     unsigned int hblank;
3423     int ret;
3424
3425     v4l2_ctrl_handler_init(hdl, 32);
3426
3427     /* we can use our own mutex for the ctrl lock */
3428     hdl->lock = &sensor->lock;
3429
3430     /* Clock related controls */
3431     ctrls->pixel_rate = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
3432                   ov5640_pixel_rates[OV5640_NUM_PIXEL_RATES - 1],
3433                   ov5640_pixel_rates[0], 1,
3434                   ov5640_pixel_rates[mode->pixel_rate]);
3435
3436     ctrls->link_freq = v4l2_ctrl_new_int_menu(hdl, ops,
3437                     V4L2_CID_LINK_FREQ,
3438                     ARRAY_SIZE(ov5640_csi2_link_freqs) - 1,
3439                     OV5640_DEFAULT_LINK_FREQ,
3440                     ov5640_csi2_link_freqs);
3441
3442     timings = ov5640_timings(sensor, mode);
3443     hblank = timings->htot - mode->width;
3444     ctrls->hblank = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HBLANK, hblank,
3445                       hblank, 1, hblank);
3446
3447     max_vblank = OV5640_MAX_VTS - mode->height;
3448     ctrls->vblank = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VBLANK,
3449                       OV5640_MIN_VBLANK, max_vblank,
3450                       1, timings->vblank_def);
3451
3452     /* Auto/manual white balance */
3453     ctrls->auto_wb = v4l2_ctrl_new_std(hdl, ops,
3454                        V4L2_CID_AUTO_WHITE_BALANCE,
3455                        0, 1, 1, 1);
3456     ctrls->blue_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BLUE_BALANCE,
3457                         0, 4095, 1, 0);
3458     ctrls->red_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_RED_BALANCE,
3459                            0, 4095, 1, 0);
3460     /* Auto/manual exposure */
3461     ctrls->auto_exp = v4l2_ctrl_new_std_menu(hdl, ops,
3462                          V4L2_CID_EXPOSURE_AUTO,
3463                          V4L2_EXPOSURE_MANUAL, 0,
3464                          V4L2_EXPOSURE_AUTO);
3465     ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
3466                         0, 65535, 1, 0);
3467     /* Auto/manual gain */
3468     ctrls->auto_gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTOGAIN,
3469                          0, 1, 1, 1);
3470     ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN,
3471                     0, 1023, 1, 0);
3472
3473     ctrls->saturation = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SATURATION,
3474                           0, 255, 1, 64);
3475     ctrls->hue = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HUE,
3476                        0, 359, 1, 0);
3477     ctrls->contrast = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_CONTRAST,
3478                         0, 255, 1, 0);
3479     ctrls->test_pattern =
3480         v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
3481                          ARRAY_SIZE(test_pattern_menu) - 1,
3482                          0, 0, test_pattern_menu);
3483     ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP,
3484                      0, 1, 1, 0);
3485     ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP,
3486                      0, 1, 1, 0);
3487
3488     ctrls->light_freq =
3489         v4l2_ctrl_new_std_menu(hdl, ops,
3490                        V4L2_CID_POWER_LINE_FREQUENCY,
3491                        V4L2_CID_POWER_LINE_FREQUENCY_AUTO, 0,
3492                        V4L2_CID_POWER_LINE_FREQUENCY_50HZ);
3493
3494     if (hdl->error) {
3495         ret = hdl->error;
3496         goto free_ctrls;
3497     }
3498
3499     ret = v4l2_fwnode_device_parse(&sensor->i2c_client->dev, &props);
3500     if (ret)
3501         goto free_ctrls;
3502
3503     if (props.rotation == 180)
3504         sensor->upside_down = true;
3505
3506     ret = v4l2_ctrl_new_fwnode_properties(hdl, ops, &props);
3507     if (ret)
3508         goto free_ctrls;
3509
3510     ctrls->pixel_rate->flags |= V4L2_CTRL_FLAG_READ_ONLY;
3511     ctrls->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
3512     ctrls->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
3513     ctrls->gain->flags |= V4L2_CTRL_FLAG_VOLATILE;
3514     ctrls->exposure->flags |= V4L2_CTRL_FLAG_VOLATILE;
3515
3516     v4l2_ctrl_auto_cluster(3, &ctrls->auto_wb, 0, false);
3517     v4l2_ctrl_auto_cluster(2, &ctrls->auto_gain, 0, true);
3518     v4l2_ctrl_auto_cluster(2, &ctrls->auto_exp, 1, true);
3519
3520     sensor->sd.ctrl_handler = hdl;
3521     return 0;
3522
3523 free_ctrls:
3524     v4l2_ctrl_handler_free(hdl);
3525     return ret;
3526 }
3527
3528 static int ov5640_enum_frame_size(struct v4l2_subdev *sd,
3529                   struct v4l2_subdev_state *sd_state,
3530                   struct v4l2_subdev_frame_size_enum *fse)
3531 {
3532     struct ov5640_dev *sensor = to_ov5640_dev(sd);
3533     u32 bpp = ov5640_code_to_bpp(sensor, fse->code);
3534     unsigned int index = fse->index;
3535
3536     if (fse->pad != 0)
3537         return -EINVAL;
3538     if (!bpp)
3539         return -EINVAL;
3540
3541     /* Only low-resolution modes are supported for 24bpp formats. */
3542     if (bpp == 24 && index >= OV5640_MODE_720P_1280_720)
3543         return -EINVAL;
3544
3545     /* FIXME: Low resolution modes don't work in 8bpp formats. */
3546     if (bpp == 8)
3547         index += OV5640_MODE_720P_1280_720;
3548
3549     if (index >= OV5640_NUM_MODES)
3550         return -EINVAL;
3551
3552     fse->min_width = ov5640_mode_data[index].width;
3553     fse->max_width = fse->min_width;
3554     fse->min_height = ov5640_mode_data[index].height;
3555     fse->max_height = fse->min_height;
3556
3557     return 0;
3558 }
3559
3560 static int ov5640_enum_frame_interval(
3561     struct v4l2_subdev *sd,
3562     struct v4l2_subdev_state *sd_state,
3563     struct v4l2_subdev_frame_interval_enum *fie)
3564 {
3565     struct ov5640_dev *sensor = to_ov5640_dev(sd);
3566     struct v4l2_fract tpf;
3567     int ret;
3568
3569     if (fie->pad != 0)
3570         return -EINVAL;
3571     if (fie->index >= OV5640_NUM_FRAMERATES)
3572         return -EINVAL;
3573
3574     tpf.numerator = 1;
3575     tpf.denominator = ov5640_framerates[fie->index];
3576
3577     ret = ov5640_try_frame_interval(sensor, &tpf,
3578                     fie->width, fie->height);
3579     if (ret < 0)
3580         return -EINVAL;
3581
3582     fie->interval = tpf;
3583     return 0;
3584 }
3585
3586 static int ov5640_g_frame_interval(struct v4l2_subdev *sd,
3587                    struct v4l2_subdev_frame_interval *fi)
3588 {
3589     struct ov5640_dev *sensor = to_ov5640_dev(sd);
3590
3591     mutex_lock(&sensor->lock);
3592     fi->interval = sensor->frame_interval;
3593     mutex_unlock(&sensor->lock);
3594
3595     return 0;
3596 }
3597
3598 static int ov5640_s_frame_interval(struct v4l2_subdev *sd,
3599                    struct v4l2_subdev_frame_interval *fi)
3600 {
3601     struct ov5640_dev *sensor = to_ov5640_dev(sd);
3602     const struct ov5640_mode_info *mode;
3603     int frame_rate, ret = 0;
3604
3605     if (fi->pad != 0)
3606         return -EINVAL;
3607
3608     mutex_lock(&sensor->lock);
3609
3610     if (sensor->streaming) {
3611         ret = -EBUSY;
3612         goto out;
3613     }
3614
3615     mode = sensor->current_mode;
3616
3617     frame_rate = ov5640_try_frame_interval(sensor, &fi->interval,
3618                            mode->width,
3619                            mode->height);
3620     if (frame_rate < 0) {
3621         /* Always return a valid frame interval value */
3622         fi->interval = sensor->frame_interval;
3623         goto out;
3624     }
3625
3626     mode = ov5640_find_mode(sensor, mode->width, mode->height, true);
3627     if (!mode) {
3628         ret = -EINVAL;
3629         goto out;
3630     }
3631
3632     if (ov5640_framerates[frame_rate] > ov5640_framerates[mode->max_fps]) {
3633         ret = -EINVAL;
3634         goto out;
3635     }
3636
3637     if (mode != sensor->current_mode ||
3638         frame_rate != sensor->current_fr) {
3639         sensor->current_fr = frame_rate;
3640         sensor->frame_interval = fi->interval;
3641         sensor->current_mode = mode;
3642         sensor->pending_mode_change = true;
3643
3644         ov5640_update_pixel_rate(sensor);
3645     }
3646 out:
3647     mutex_unlock(&sensor->lock);
3648     return ret;
3649 }
3650
3651 static int ov5640_enum_mbus_code(struct v4l2_subdev *sd,
3652                  struct v4l2_subdev_state *sd_state,
3653                  struct v4l2_subdev_mbus_code_enum *code)
3654 {
3655     struct ov5640_dev *sensor = to_ov5640_dev(sd);
3656     const struct ov5640_pixfmt *formats;
3657     unsigned int num_formats;
3658
3659     if (ov5640_is_csi2(sensor)) {
3660         formats = ov5640_csi2_formats;
3661         num_formats = ARRAY_SIZE(ov5640_csi2_formats) - 1;
3662     } else {
3663         formats = ov5640_dvp_formats;
3664         num_formats = ARRAY_SIZE(ov5640_dvp_formats) - 1;
3665     }
3666
3667     if (code->index >= num_formats)
3668         return -EINVAL;
3669
3670     code->code = formats[code->index].code;
3671
3672     return 0;
3673 }
3674
3675 static int ov5640_s_stream(struct v4l2_subdev *sd, int enable)
3676 {
3677     struct ov5640_dev *sensor = to_ov5640_dev(sd);
3678     int ret = 0;
3679
3680     mutex_lock(&sensor->lock);
3681
3682     if (sensor->streaming == !enable) {
3683         if (enable && sensor->pending_mode_change) {
3684             ret = ov5640_set_mode(sensor);
3685             if (ret)
3686                 goto out;
3687         }
3688
3689         if (enable && sensor->pending_fmt_change) {
3690             ret = ov5640_set_framefmt(sensor, &sensor->fmt);
3691             if (ret)
3692                 goto out;
3693             sensor->pending_fmt_change = false;
3694         }
3695
3696         if (ov5640_is_csi2(sensor))
3697             ret = ov5640_set_stream_mipi(sensor, enable);
3698         else
3699             ret = ov5640_set_stream_dvp(sensor, enable);
3700
3701         if (!ret)
3702             sensor->streaming = enable;
3703     }
3704 out:
3705     mutex_unlock(&sensor->lock);
3706     return ret;
3707 }
3708
3709 static int ov5640_init_cfg(struct v4l2_subdev *sd,
3710                struct v4l2_subdev_state *state)
3711 {
3712     struct v4l2_mbus_framefmt *fmt =
3713                 v4l2_subdev_get_try_format(sd, state, 0);
3714     struct v4l2_rect *crop = v4l2_subdev_get_try_crop(sd, state, 0);
3715
3716     *fmt = ov5640_default_fmt;
3717
3718     crop->left = OV5640_PIXEL_ARRAY_LEFT;
3719     crop->top = OV5640_PIXEL_ARRAY_TOP;
3720     crop->width = OV5640_PIXEL_ARRAY_WIDTH;
3721     crop->height = OV5640_PIXEL_ARRAY_HEIGHT;
3722
3723     return 0;
3724 }
3725
3726 static const struct v4l2_subdev_core_ops ov5640_core_ops = {
3727     .s_power = ov5640_s_power,
3728     .log_status = v4l2_ctrl_subdev_log_status,
3729     .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
3730     .unsubscribe_event = v4l2_event_subdev_unsubscribe,
3731 };
3732
3733 static const struct v4l2_subdev_video_ops ov5640_video_ops = {
3734     .g_frame_interval = ov5640_g_frame_interval,
3735     .s_frame_interval = ov5640_s_frame_interval,
3736     .s_stream = ov5640_s_stream,
3737 };
3738
3739 static const struct v4l2_subdev_pad_ops ov5640_pad_ops = {
3740     .init_cfg = ov5640_init_cfg,
3741     .enum_mbus_code = ov5640_enum_mbus_code,
3742     .get_fmt = ov5640_get_fmt,
3743     .set_fmt = ov5640_set_fmt,
3744     .get_selection = ov5640_get_selection,
3745     .enum_frame_size = ov5640_enum_frame_size,
3746     .enum_frame_interval = ov5640_enum_frame_interval,
3747 };
3748
3749 static const struct v4l2_subdev_ops ov5640_subdev_ops = {
3750     .core = &ov5640_core_ops,
3751     .video = &ov5640_video_ops,
3752     .pad = &ov5640_pad_ops,
3753 };
3754
3755 static int ov5640_get_regulators(struct ov5640_dev *sensor)
3756 {
3757     int i;
3758
3759     for (i = 0; i < OV5640_NUM_SUPPLIES; i++)
3760         sensor->supplies[i].supply = ov5640_supply_name[i];
3761
3762     return devm_regulator_bulk_get(&sensor->i2c_client->dev,
3763                        OV5640_NUM_SUPPLIES,
3764                        sensor->supplies);
3765 }
3766
3767 static int ov5640_check_chip_id(struct ov5640_dev *sensor)
3768 {
3769     struct i2c_client *client = sensor->i2c_client;
3770     int ret = 0;
3771     u16 chip_id;
3772
3773     ret = ov5640_set_power_on(sensor);
3774     if (ret)
3775         return ret;
3776
3777     ret = ov5640_read_reg16(sensor, OV5640_REG_CHIP_ID, &chip_id);
3778     if (ret) {
3779         dev_err(&client->dev, "%s: failed to read chip identifier\n",
3780             __func__);
3781         goto power_off;
3782     }
3783
3784     if (chip_id != 0x5640) {
3785         dev_err(&client->dev, "%s: wrong chip identifier, expected 0x5640, got 0x%x\n",
3786             __func__, chip_id);
3787         ret = -ENXIO;
3788     }
3789
3790 power_off:
3791     ov5640_set_power_off(sensor);
3792     return ret;
3793 }
3794
3795 static int ov5640_probe(struct i2c_client *client)
3796 {
3797     struct device *dev = &client->dev;
3798     struct fwnode_handle *endpoint;
3799     struct ov5640_dev *sensor;
3800     int ret;
3801
3802     sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
3803     if (!sensor)
3804         return -ENOMEM;
3805
3806     sensor->i2c_client = client;
3807
3808     /*
3809      * default init sequence initialize sensor to
3810      * YUV422 UYVY VGA@30fps
3811      */
3812     sensor->fmt = ov5640_default_fmt;
3813     sensor->frame_interval.numerator = 1;
3814     sensor->frame_interval.denominator = ov5640_framerates[OV5640_30_FPS];
3815     sensor->current_fr = OV5640_30_FPS;
3816     sensor->current_mode =
3817         &ov5640_mode_data[OV5640_MODE_VGA_640_480];
3818     sensor->last_mode = sensor->current_mode;
3819     sensor->current_link_freq = OV5640_DEFAULT_LINK_FREQ;
3820
3821     sensor->ae_target = 52;
3822
3823     endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(&client->dev),
3824                           NULL);
3825     if (!endpoint) {
3826         dev_err(dev, "endpoint node not found\n");
3827         return -EINVAL;
3828     }
3829
3830     ret = v4l2_fwnode_endpoint_parse(endpoint, &sensor->ep);
3831     fwnode_handle_put(endpoint);
3832     if (ret) {
3833         dev_err(dev, "Could not parse endpoint\n");
3834         return ret;
3835     }
3836
3837     if (sensor->ep.bus_type != V4L2_MBUS_PARALLEL &&
3838         sensor->ep.bus_type != V4L2_MBUS_CSI2_DPHY &&
3839         sensor->ep.bus_type != V4L2_MBUS_BT656) {
3840         dev_err(dev, "Unsupported bus type %d\n", sensor->ep.bus_type);
3841         return -EINVAL;
3842     }
3843
3844     /* get system clock (xclk) */
3845     sensor->xclk = devm_clk_get(dev, "xclk");
3846     if (IS_ERR(sensor->xclk)) {
3847         dev_err(dev, "failed to get xclk\n");
3848         return PTR_ERR(sensor->xclk);
3849     }
3850
3851     sensor->xclk_freq = clk_get_rate(sensor->xclk);
3852     if (sensor->xclk_freq < OV5640_XCLK_MIN ||
3853         sensor->xclk_freq > OV5640_XCLK_MAX) {
3854         dev_err(dev, "xclk frequency out of range: %d Hz\n",
3855             sensor->xclk_freq);
3856         return -EINVAL;
3857     }
3858
3859     /* request optional power down pin */
3860     sensor->pwdn_gpio = devm_gpiod_get_optional(dev, "powerdown",
3861                             GPIOD_OUT_HIGH);
3862     if (IS_ERR(sensor->pwdn_gpio))
3863         return PTR_ERR(sensor->pwdn_gpio);
3864
3865     /* request optional reset pin */
3866     sensor->reset_gpio = devm_gpiod_get_optional(dev, "reset",
3867                              GPIOD_OUT_HIGH);
3868     if (IS_ERR(sensor->reset_gpio))
3869         return PTR_ERR(sensor->reset_gpio);
3870
3871     v4l2_i2c_subdev_init(&sensor->sd, client, &ov5640_subdev_ops);
3872
3873     sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
3874                 V4L2_SUBDEV_FL_HAS_EVENTS;
3875     sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
3876     sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
3877     ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
3878     if (ret)
3879         return ret;
3880
3881     ret = ov5640_get_regulators(sensor);
3882     if (ret)
3883         return ret;
3884
3885     mutex_init(&sensor->lock);
3886
3887     ret = ov5640_check_chip_id(sensor);
3888     if (ret)
3889         goto entity_cleanup;
3890
3891     ret = ov5640_init_controls(sensor);
3892     if (ret)
3893         goto entity_cleanup;
3894
3895     ret = v4l2_async_register_subdev_sensor(&sensor->sd);
3896     if (ret)
3897         goto free_ctrls;
3898
3899     return 0;
3900
3901 free_ctrls:
3902     v4l2_ctrl_handler_free(&sensor->ctrls.handler);
3903 entity_cleanup:
3904     media_entity_cleanup(&sensor->sd.entity);
3905     mutex_destroy(&sensor->lock);
3906     return ret;
3907 }
3908
3909 static int ov5640_remove(struct i2c_client *client)
3910 {
3911     struct v4l2_subdev *sd = i2c_get_clientdata(client);
3912     struct ov5640_dev *sensor = to_ov5640_dev(sd);
3913
3914     v4l2_async_unregister_subdev(&sensor->sd);
3915     media_entity_cleanup(&sensor->sd.entity);
3916     v4l2_ctrl_handler_free(&sensor->ctrls.handler);
3917     mutex_destroy(&sensor->lock);
3918
3919     return 0;
3920 }
3921
3922 static const struct i2c_device_id ov5640_id[] = {
3923     {"ov5640", 0},
3924     {},
3925 };
3926 MODULE_DEVICE_TABLE(i2c, ov5640_id);
3927
3928 static const struct of_device_id ov5640_dt_ids[] = {
3929     { .compatible = "ovti,ov5640" },
3930     { /* sentinel */ }
3931 };
3932 MODULE_DEVICE_TABLE(of, ov5640_dt_ids);
3933
3934 static struct i2c_driver ov5640_i2c_driver = {
3935     .driver = {
3936         .name  = "ov5640",
3937         .of_match_table = ov5640_dt_ids,
3938     },
3939     .id_table = ov5640_id,
3940     .probe_new = ov5640_probe,
3941     .remove   = ov5640_remove,
3942 };
3943
3944 module_i2c_driver(ov5640_i2c_driver);
3945
3946 MODULE_DESCRIPTION("OV5640 MIPI Camera Subdev Driver");
3947 MODULE_LICENSE("GPL");