drivers/media/i2c/ov9650.c

   1 /*
   2  * Omnivision OV9650/OV9652 CMOS Image Sensor driver
   3  *
   4  * Copyright (C) 2013, Sylwester Nawrocki <sylvester.nawrocki@gmail.com>
   5  *
   6  * Register definitions and initial settings based on a driver written
   7  * by Vladimir Fonov.
   8  * Copyright (c) 2010, Vladimir Fonov
   9  *
  10  * This program is free software; you can redistribute it and/or modify
  11  * it under the terms of the GNU General Public License version 2 as
  12  * published by the Free Software Foundation.
  13  */
  14 #include <linux/clk.h>
  15 #include <linux/delay.h>
  16 #include <linux/gpio.h>
  17 #include <linux/gpio/consumer.h>
  18 #include <linux/i2c.h>
  19 #include <linux/kernel.h>
  20 #include <linux/media.h>
  21 #include <linux/module.h>
  22 #include <linux/ratelimit.h>
  23 #include <linux/slab.h>
  24 #include <linux/string.h>
  25 #include <linux/videodev2.h>
  26
  27 #include <media/media-entity.h>
  28 #include <media/v4l2-async.h>
  29 #include <media/v4l2-ctrls.h>
  30 #include <media/v4l2-device.h>
  31 #include <media/v4l2-event.h>
  32 #include <media/v4l2-image-sizes.h>
  33 #include <media/v4l2-subdev.h>
  34 #include <media/v4l2-mediabus.h>
  35 #include <media/i2c/ov9650.h>
  36
  37 static int debug;
  38 module_param(debug, int, 0644);
  39 MODULE_PARM_DESC(debug, "Debug level (0-2)");
  40
  41 #define DRIVER_NAME "OV9650"
  42
  43 /*
  44  * OV9650/OV9652 register definitions
  45  */
  46 #define REG_GAIN                0x00    /* Gain control, AGC[7:0] */
  47 #define REG_BLUE                0x01    /* AWB - Blue chanel gain */
  48 #define REG_RED                 0x02    /* AWB - Red chanel gain */
  49 #define REG_VREF                0x03    /* [7:6] - AGC[9:8], [5:3]/[2:0] */
  50 #define  VREF_GAIN_MASK         0xc0    /* - VREF end/start low 3 bits */
  51 #define REG_COM1                0x04
  52 #define  COM1_CCIR656           0x40
  53 #define REG_B_AVE               0x05
  54 #define REG_GB_AVE              0x06
  55 #define REG_GR_AVE              0x07
  56 #define REG_R_AVE               0x08
  57 #define REG_COM2                0x09
  58 #define REG_PID                 0x0a    /* Product ID MSB */
  59 #define REG_VER                 0x0b    /* Product ID LSB */
  60 #define REG_COM3                0x0c
  61 #define  COM3_SWAP              0x40
  62 #define  COM3_VARIOPIXEL1       0x04
  63 #define REG_COM4                0x0d    /* Vario Pixels  */
  64 #define  COM4_VARIOPIXEL2       0x80
  65 #define REG_COM5                0x0e    /* System clock options */
  66 #define  COM5_SLAVE_MODE        0x10
  67 #define  COM5_SYSTEMCLOCK48MHZ  0x80
  68 #define REG_COM6                0x0f    /* HREF & ADBLC options */
  69 #define REG_AECH                0x10    /* Exposure value, AEC[9:2] */
  70 #define REG_CLKRC               0x11    /* Clock control */
  71 #define  CLK_EXT                0x40    /* Use external clock directly */
  72 #define  CLK_SCALE              0x3f    /* Mask for internal clock scale */
  73 #define REG_COM7                0x12    /* SCCB reset, output format */
  74 #define  COM7_RESET             0x80
  75 #define  COM7_FMT_MASK          0x38
  76 #define  COM7_FMT_VGA           0x40
  77 #define  COM7_FMT_CIF           0x20
  78 #define  COM7_FMT_QVGA          0x10
  79 #define  COM7_FMT_QCIF          0x08
  80 #define  COM7_RGB               0x04
  81 #define  COM7_YUV               0x00
  82 #define  COM7_BAYER             0x01
  83 #define  COM7_PBAYER            0x05
  84 #define REG_COM8                0x13    /* AGC/AEC options */
  85 #define  COM8_FASTAEC           0x80    /* Enable fast AGC/AEC */
  86 #define  COM8_AECSTEP           0x40    /* Unlimited AEC step size */
  87 #define  COM8_BFILT             0x20    /* Band filter enable */
  88 #define  COM8_AGC               0x04    /* Auto gain enable */
  89 #define  COM8_AWB               0x02    /* White balance enable */
  90 #define  COM8_AEC               0x01    /* Auto exposure enable */
  91 #define REG_COM9                0x14    /* Gain ceiling */
  92 #define  COM9_GAIN_CEIL_MASK    0x70    /* */
  93 #define REG_COM10               0x15    /* PCLK, HREF, HSYNC signals polarity */
  94 #define  COM10_HSYNC            0x40    /* HSYNC instead of HREF */
  95 #define  COM10_PCLK_HB          0x20    /* Suppress PCLK on horiz blank */
  96 #define  COM10_HREF_REV         0x08    /* Reverse HREF */
  97 #define  COM10_VS_LEAD          0x04    /* VSYNC on clock leading edge */
  98 #define  COM10_VS_NEG           0x02    /* VSYNC negative */
  99 #define  COM10_HS_NEG           0x01    /* HSYNC negative */
 100 #define REG_HSTART              0x17    /* Horiz start high bits */
 101 #define REG_HSTOP               0x18    /* Horiz stop high bits */
 102 #define REG_VSTART              0x19    /* Vert start high bits */
 103 #define REG_VSTOP               0x1a    /* Vert stop high bits */
 104 #define REG_PSHFT               0x1b    /* Pixel delay after HREF */
 105 #define REG_MIDH                0x1c    /* Manufacturer ID MSB */
 106 #define REG_MIDL                0x1d    /* Manufufacturer ID LSB */
 107 #define REG_MVFP                0x1e    /* Image mirror/flip */
 108 #define  MVFP_MIRROR            0x20    /* Mirror image */
 109 #define  MVFP_FLIP              0x10    /* Vertical flip */
 110 #define REG_BOS                 0x20    /* B channel Offset */
 111 #define REG_GBOS                0x21    /* Gb channel Offset */
 112 #define REG_GROS                0x22    /* Gr channel Offset */
 113 #define REG_ROS                 0x23    /* R channel Offset */
 114 #define REG_AEW                 0x24    /* AGC upper limit */
 115 #define REG_AEB                 0x25    /* AGC lower limit */
 116 #define REG_VPT                 0x26    /* AGC/AEC fast mode op region */
 117 #define REG_BBIAS               0x27    /* B channel output bias */
 118 #define REG_GBBIAS              0x28    /* Gb channel output bias */
 119 #define REG_GRCOM               0x29    /* Analog BLC & regulator */
 120 #define REG_EXHCH               0x2a    /* Dummy pixel insert MSB */
 121 #define REG_EXHCL               0x2b    /* Dummy pixel insert LSB */
 122 #define REG_RBIAS               0x2c    /* R channel output bias */
 123 #define REG_ADVFL               0x2d    /* LSB of dummy line insert */
 124 #define REG_ADVFH               0x2e    /* MSB of dummy line insert */
 125 #define REG_YAVE                0x2f    /* Y/G channel average value */
 126 #define REG_HSYST               0x30    /* HSYNC rising edge delay LSB*/
 127 #define REG_HSYEN               0x31    /* HSYNC falling edge delay LSB*/
 128 #define REG_HREF                0x32    /* HREF pieces */
 129 #define REG_CHLF                0x33    /* reserved */
 130 #define REG_ADC                 0x37    /* reserved */
 131 #define REG_ACOM                0x38    /* reserved */
 132 #define REG_OFON                0x39    /* Power down register */
 133 #define  OFON_PWRDN             0x08    /* Power down bit */
 134 #define REG_TSLB                0x3a    /* YUVU format */
 135 #define  TSLB_YUYV_MASK         0x0c    /* UYVY or VYUY - see com13 */
 136 #define REG_COM11               0x3b    /* Night mode, banding filter enable */
 137 #define  COM11_NIGHT            0x80    /* Night mode enable */
 138 #define  COM11_NMFR             0x60    /* Two bit NM frame rate */
 139 #define  COM11_BANDING          0x01    /* Banding filter */
 140 #define  COM11_AEC_REF_MASK     0x18    /* AEC reference area selection */
 141 #define REG_COM12               0x3c    /* HREF option, UV average */
 142 #define  COM12_HREF             0x80    /* HREF always */
 143 #define REG_COM13               0x3d    /* Gamma selection, Color matrix en. */
 144 #define  COM13_GAMMA            0x80    /* Gamma enable */
 145 #define  COM13_UVSAT            0x40    /* UV saturation auto adjustment */
 146 #define  COM13_UVSWAP           0x01    /* V before U - w/TSLB */
 147 #define REG_COM14               0x3e    /* Edge enhancement options */
 148 #define  COM14_EDGE_EN          0x02
 149 #define  COM14_EEF_X2           0x01
 150 #define REG_EDGE                0x3f    /* Edge enhancement factor */
 151 #define  EDGE_FACTOR_MASK       0x0f
 152 #define REG_COM15               0x40    /* Output range, RGB 555/565 */
 153 #define  COM15_R10F0            0x00    /* Data range 10 to F0 */
 154 #define  COM15_R01FE            0x80    /* 01 to FE */
 155 #define  COM15_R00FF            0xc0    /* 00 to FF */
 156 #define  COM15_RGB565           0x10    /* RGB565 output */
 157 #define  COM15_RGB555           0x30    /* RGB555 output */
 158 #define  COM15_SWAPRB           0x04    /* Swap R&B */
 159 #define REG_COM16               0x41    /* Color matrix coeff options */
 160 #define REG_COM17               0x42    /* Single frame out, banding filter */
 161 /* n = 1...9, 0x4f..0x57 */
 162 #define REG_MTX(__n)            (0x4f + (__n) - 1)
 163 #define REG_MTXS                0x58
 164 /* Lens Correction Option 1...5, __n = 0...5 */
 165 #define REG_LCC(__n)            (0x62 + (__n) - 1)
 166 #define  LCC5_LCC_ENABLE        0x01    /* LCC5, enable lens correction */
 167 #define  LCC5_LCC_COLOR         0x04
 168 #define REG_MANU                0x67    /* Manual U value */
 169 #define REG_MANV                0x68    /* Manual V value */
 170 #define REG_HV                  0x69    /* Manual banding filter MSB */
 171 #define REG_MBD                 0x6a    /* Manual banding filter value */
 172 #define REG_DBLV                0x6b    /* reserved */
 173 #define REG_GSP                 0x6c    /* Gamma curve */
 174 #define  GSP_LEN                15
 175 #define REG_GST                 0x7c    /* Gamma curve */
 176 #define  GST_LEN                15
 177 #define REG_COM21               0x8b
 178 #define REG_COM22               0x8c    /* Edge enhancement, denoising */
 179 #define  COM22_WHTPCOR          0x02    /* White pixel correction enable */
 180 #define  COM22_WHTPCOROPT       0x01    /* White pixel correction option */
 181 #define  COM22_DENOISE          0x10    /* White pixel correction option */
 182 #define REG_COM23               0x8d    /* Color bar test, color gain */
 183 #define  COM23_TEST_MODE        0x10
 184 #define REG_DBLC1               0x8f    /* Digital BLC */
 185 #define REG_DBLC_B              0x90    /* Digital BLC B channel offset */
 186 #define REG_DBLC_R              0x91    /* Digital BLC R channel offset */
 187 #define REG_DM_LNL              0x92    /* Dummy line low 8 bits */
 188 #define REG_DM_LNH              0x93    /* Dummy line high 8 bits */
 189 #define REG_LCCFB               0x9d    /* Lens Correction B channel */
 190 #define REG_LCCFR               0x9e    /* Lens Correction R channel */
 191 #define REG_DBLC_GB             0x9f    /* Digital BLC GB chan offset */
 192 #define REG_DBLC_GR             0xa0    /* Digital BLC GR chan offset */
 193 #define REG_AECHM               0xa1    /* Exposure value - bits AEC[15:10] */
 194 #define REG_BD50ST              0xa2    /* Banding filter value for 50Hz */
 195 #define REG_BD60ST              0xa3    /* Banding filter value for 60Hz */
 196 #define REG_NULL                0xff    /* Array end token */
 197
 198 #define DEF_CLKRC               0x80
 199
 200 #define OV965X_ID(_msb, _lsb)   ((_msb) << 8 | (_lsb))
 201 #define OV9650_ID               0x9650
 202 #define OV9652_ID               0x9652
 203
 204 struct ov965x_ctrls {
 205         struct v4l2_ctrl_handler handler;
 206         struct {
 207                 struct v4l2_ctrl *auto_exp;
 208                 struct v4l2_ctrl *exposure;
 209         };
 210         struct {
 211                 struct v4l2_ctrl *auto_wb;
 212                 struct v4l2_ctrl *blue_balance;
 213                 struct v4l2_ctrl *red_balance;
 214         };
 215         struct {
 216                 struct v4l2_ctrl *hflip;
 217                 struct v4l2_ctrl *vflip;
 218         };
 219         struct {
 220                 struct v4l2_ctrl *auto_gain;
 221                 struct v4l2_ctrl *gain;
 222         };
 223         struct v4l2_ctrl *brightness;
 224         struct v4l2_ctrl *saturation;
 225         struct v4l2_ctrl *sharpness;
 226         struct v4l2_ctrl *light_freq;
 227         u8 update;
 228 };
 229
 230 struct ov965x_framesize {
 231         u16 width;
 232         u16 height;
 233         u16 max_exp_lines;
 234         const u8 *regs;
 235 };
 236
 237 struct ov965x_interval {
 238         struct v4l2_fract interval;
 239         /* Maximum resolution for this interval */
 240         struct v4l2_frmsize_discrete size;
 241         u8 clkrc_div;
 242 };
 243
 244 enum gpio_id {
 245         GPIO_PWDN,
 246         GPIO_RST,
 247         NUM_GPIOS,
 248 };
 249
 250 struct ov965x {
 251         struct v4l2_subdev sd;
 252         struct media_pad pad;
 253         enum v4l2_mbus_type bus_type;
 254         struct gpio_desc *gpios[NUM_GPIOS];
 255         /* External master clock frequency */
 256         unsigned long mclk_frequency;
 257         struct clk *clk;
 258
 259         /* Protects the struct fields below */
 260         struct mutex lock;
 261
 262         struct i2c_client *client;
 263
 264         /* Exposure row interval in us */
 265         unsigned int exp_row_interval;
 266
 267         unsigned short id;
 268         const struct ov965x_framesize *frame_size;
 269         /* YUYV sequence (pixel format) control register */
 270         u8 tslb_reg;
 271         struct v4l2_mbus_framefmt format;
 272
 273         struct ov965x_ctrls ctrls;
 274         /* Pointer to frame rate control data structure */
 275         const struct ov965x_interval *fiv;
 276
 277         int streaming;
 278         int power;
 279
 280         u8 apply_frame_fmt;
 281 };
 282
 283 struct i2c_rv {
 284         u8 addr;
 285         u8 value;
 286 };
 287
 288 static const struct i2c_rv ov965x_init_regs[] = {
 289         { REG_COM2, 0x10 },     /* Set soft sleep mode */
 290         { REG_COM5, 0x00 },     /* System clock options */
 291         { REG_COM2, 0x01 },     /* Output drive, soft sleep mode */
 292         { REG_COM10, 0x00 },    /* Slave mode, HREF vs HSYNC, signals negate */
 293         { REG_EDGE, 0xa6 },     /* Edge enhancement treshhold and factor */
 294         { REG_COM16, 0x02 },    /* Color matrix coeff double option */
 295         { REG_COM17, 0x08 },    /* Single frame out, banding filter */
 296         { 0x16, 0x06 },
 297         { REG_CHLF, 0xc0 },     /* Reserved  */
 298         { 0x34, 0xbf },
 299         { 0xa8, 0x80 },
 300         { 0x96, 0x04 },
 301         { 0x8e, 0x00 },
 302         { REG_COM12, 0x77 },    /* HREF option, UV average  */
 303         { 0x8b, 0x06 },
 304         { 0x35, 0x91 },
 305         { 0x94, 0x88 },
 306         { 0x95, 0x88 },
 307         { REG_COM15, 0xc1 },    /* Output range, RGB 555/565 */
 308         { REG_GRCOM, 0x2f },    /* Analog BLC & regulator */
 309         { REG_COM6, 0x43 },     /* HREF & ADBLC options */
 310         { REG_COM8, 0xe5 },     /* AGC/AEC options */
 311         { REG_COM13, 0x90 },    /* Gamma selection, colour matrix, UV delay */
 312         { REG_HV, 0x80 },       /* Manual banding filter MSB  */
 313         { 0x5c, 0x96 },         /* Reserved up to 0xa5 */
 314         { 0x5d, 0x96 },
 315         { 0x5e, 0x10 },
 316         { 0x59, 0xeb },
 317         { 0x5a, 0x9c },
 318         { 0x5b, 0x55 },
 319         { 0x43, 0xf0 },
 320         { 0x44, 0x10 },
 321         { 0x45, 0x55 },
 322         { 0x46, 0x86 },
 323         { 0x47, 0x64 },
 324         { 0x48, 0x86 },
 325         { 0x5f, 0xe0 },
 326         { 0x60, 0x8c },
 327         { 0x61, 0x20 },
 328         { 0xa5, 0xd9 },
 329         { 0xa4, 0x74 },         /* reserved */
 330         { REG_COM23, 0x02 },    /* Color gain analog/_digital_ */
 331         { REG_COM8, 0xe7 },     /* Enable AEC, AWB, AEC */
 332         { REG_COM22, 0x23 },    /* Edge enhancement, denoising */
 333         { 0xa9, 0xb8 },
 334         { 0xaa, 0x92 },
 335         { 0xab, 0x0a },
 336         { REG_DBLC1, 0xdf },    /* Digital BLC */
 337         { REG_DBLC_B, 0x00 },   /* Digital BLC B chan offset */
 338         { REG_DBLC_R, 0x00 },   /* Digital BLC R chan offset */
 339         { REG_DBLC_GB, 0x00 },  /* Digital BLC GB chan offset */
 340         { REG_DBLC_GR, 0x00 },
 341         { REG_COM9, 0x3a },     /* Gain ceiling 16x */
 342         { REG_NULL, 0 }
 343 };
 344
 345 #define NUM_FMT_REGS 14
 346 /*
 347  * COM7,  COM3,  COM4, HSTART, HSTOP, HREF, VSTART, VSTOP, VREF,
 348  * EXHCH, EXHCL, ADC,  OCOM,   OFON
 349  */
 350 static const u8 frame_size_reg_addr[NUM_FMT_REGS] = {
 351         0x12, 0x0c, 0x0d, 0x17, 0x18, 0x32, 0x19, 0x1a, 0x03,
 352         0x2a, 0x2b, 0x37, 0x38, 0x39,
 353 };
 354
 355 static const u8 ov965x_sxga_regs[NUM_FMT_REGS] = {
 356         0x00, 0x00, 0x00, 0x1e, 0xbe, 0xbf, 0x01, 0x81, 0x12,
 357         0x10, 0x34, 0x81, 0x93, 0x51,
 358 };
 359
 360 static const u8 ov965x_vga_regs[NUM_FMT_REGS] = {
 361         0x40, 0x04, 0x80, 0x26, 0xc6, 0xed, 0x01, 0x3d, 0x00,
 362         0x10, 0x40, 0x91, 0x12, 0x43,
 363 };
 364
 365 /* Determined empirically. */
 366 static const u8 ov965x_qvga_regs[NUM_FMT_REGS] = {
 367         0x10, 0x04, 0x80, 0x25, 0xc5, 0xbf, 0x00, 0x80, 0x12,
 368         0x10, 0x40, 0x91, 0x12, 0x43,
 369 };
 370
 371 static const struct ov965x_framesize ov965x_framesizes[] = {
 372         {
 373                 .width          = SXGA_WIDTH,
 374                 .height         = SXGA_HEIGHT,
 375                 .regs           = ov965x_sxga_regs,
 376                 .max_exp_lines  = 1048,
 377         }, {
 378                 .width          = VGA_WIDTH,
 379                 .height         = VGA_HEIGHT,
 380                 .regs           = ov965x_vga_regs,
 381                 .max_exp_lines  = 498,
 382         }, {
 383                 .width          = QVGA_WIDTH,
 384                 .height         = QVGA_HEIGHT,
 385                 .regs           = ov965x_qvga_regs,
 386                 .max_exp_lines  = 248,
 387         },
 388 };
 389
 390 struct ov965x_pixfmt {
 391         u32 code;
 392         u32 colorspace;
 393         /* REG_TSLB value, only bits [3:2] may be set. */
 394         u8 tslb_reg;
 395 };
 396
 397 static const struct ov965x_pixfmt ov965x_formats[] = {
 398         { MEDIA_BUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_JPEG, 0x00},
 399         { MEDIA_BUS_FMT_YVYU8_2X8, V4L2_COLORSPACE_JPEG, 0x04},
 400         { MEDIA_BUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_JPEG, 0x0c},
 401         { MEDIA_BUS_FMT_VYUY8_2X8, V4L2_COLORSPACE_JPEG, 0x08},
 402 };
 403
 404 /*
 405  * This table specifies possible frame resolution and interval
 406  * combinations. Default CLKRC[5:0] divider values are valid
 407  * only for 24 MHz external clock frequency.
 408  */
 409 static struct ov965x_interval ov965x_intervals[] = {
 410         {{ 100, 625 }, { SXGA_WIDTH, SXGA_HEIGHT }, 0 },  /* 6.25 fps */
 411         {{ 10,  125 }, { VGA_WIDTH, VGA_HEIGHT },   1 },  /* 12.5 fps */
 412         {{ 10,  125 }, { QVGA_WIDTH, QVGA_HEIGHT }, 3 },  /* 12.5 fps */
 413         {{ 1,   25  }, { VGA_WIDTH, VGA_HEIGHT },   0 },  /* 25 fps */
 414         {{ 1,   25  }, { QVGA_WIDTH, QVGA_HEIGHT }, 1 },  /* 25 fps */
 415 };
 416
 417 static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
 418 {
 419         return &container_of(ctrl->handler, struct ov965x, ctrls.handler)->sd;
 420 }
 421
 422 static inline struct ov965x *to_ov965x(struct v4l2_subdev *sd)
 423 {
 424         return container_of(sd, struct ov965x, sd);
 425 }
 426
 427 static int ov965x_read(struct i2c_client *client, u8 addr, u8 *val)
 428 {
 429         u8 buf = addr;
 430         struct i2c_msg msg = {
 431                 .addr = client->addr,
 432                 .flags = 0,
 433                 .len = 1,
 434                 .buf = &buf
 435         };
 436         int ret;
 437
 438         ret = i2c_transfer(client->adapter, &msg, 1);
 439         if (ret == 1) {
 440                 msg.flags = I2C_M_RD;
 441                 ret = i2c_transfer(client->adapter, &msg, 1);
 442
 443                 if (ret == 1)
 444                         *val = buf;
 445         }
 446
 447         v4l2_dbg(2, debug, client, "%s: 0x%02x @ 0x%02x. (%d)\n",
 448                  __func__, *val, addr, ret);
 449
 450         return ret == 1 ? 0 : ret;
 451 }
 452
 453 static int ov965x_write(struct i2c_client *client, u8 addr, u8 val)
 454 {
 455         u8 buf[2] = { addr, val };
 456
 457         int ret = i2c_master_send(client, buf, 2);
 458
 459         v4l2_dbg(2, debug, client, "%s: 0x%02x @ 0x%02X (%d)\n",
 460                  __func__, val, addr, ret);
 461
 462         return ret == 2 ? 0 : ret;
 463 }
 464
 465 static int ov965x_write_array(struct i2c_client *client,
 466                               const struct i2c_rv *regs)
 467 {
 468         int i, ret = 0;
 469
 470         for (i = 0; ret == 0 && regs[i].addr != REG_NULL; i++)
 471                 ret = ov965x_write(client, regs[i].addr, regs[i].value);
 472
 473         return ret;
 474 }
 475
 476 static int ov965x_set_default_gamma_curve(struct ov965x *ov965x)
 477 {
 478         static const u8 gamma_curve[] = {
 479                 /* Values taken from OV application note. */
 480                 0x40, 0x30, 0x4b, 0x60, 0x70, 0x70, 0x70, 0x70,
 481                 0x60, 0x60, 0x50, 0x48, 0x3a, 0x2e, 0x28, 0x22,
 482                 0x04, 0x07, 0x10, 0x28, 0x36, 0x44, 0x52, 0x60,
 483                 0x6c, 0x78, 0x8c, 0x9e, 0xbb, 0xd2, 0xe6
 484         };
 485         u8 addr = REG_GSP;
 486         unsigned int i;
 487
 488         for (i = 0; i < ARRAY_SIZE(gamma_curve); i++) {
 489                 int ret = ov965x_write(ov965x->client, addr, gamma_curve[i]);
 490
 491                 if (ret < 0)
 492                         return ret;
 493                 addr++;
 494         }
 495
 496         return 0;
 497 };
 498
 499 static int ov965x_set_color_matrix(struct ov965x *ov965x)
 500 {
 501         static const u8 mtx[] = {
 502                 /* MTX1..MTX9, MTXS */
 503                 0x3a, 0x3d, 0x03, 0x12, 0x26, 0x38, 0x40, 0x40, 0x40, 0x0d
 504         };
 505         u8 addr = REG_MTX(1);
 506         unsigned int i;
 507
 508         for (i = 0; i < ARRAY_SIZE(mtx); i++) {
 509                 int ret = ov965x_write(ov965x->client, addr, mtx[i]);
 510
 511                 if (ret < 0)
 512                         return ret;
 513                 addr++;
 514         }
 515
 516         return 0;
 517 }
 518
 519 static int __ov965x_set_power(struct ov965x *ov965x, int on)
 520 {
 521         if (on) {
 522                 int ret = clk_prepare_enable(ov965x->clk);
 523
 524                 if (ret)
 525                         return ret;
 526
 527                 gpiod_set_value_cansleep(ov965x->gpios[GPIO_PWDN], 0);
 528                 gpiod_set_value_cansleep(ov965x->gpios[GPIO_RST], 0);
 529                 msleep(25);
 530         } else {
 531                 gpiod_set_value_cansleep(ov965x->gpios[GPIO_RST], 1);
 532                 gpiod_set_value_cansleep(ov965x->gpios[GPIO_PWDN], 1);
 533
 534                 clk_disable_unprepare(ov965x->clk);
 535         }
 536
 537         ov965x->streaming = 0;
 538
 539         return 0;
 540 }
 541
 542 static int ov965x_s_power(struct v4l2_subdev *sd, int on)
 543 {
 544         struct ov965x *ov965x = to_ov965x(sd);
 545         struct i2c_client *client = ov965x->client;
 546         int ret = 0;
 547
 548         v4l2_dbg(1, debug, client, "%s: on: %d\n", __func__, on);
 549
 550         mutex_lock(&ov965x->lock);
 551         if (ov965x->power == !on) {
 552                 ret = __ov965x_set_power(ov965x, on);
 553                 if (!ret && on) {
 554                         ret = ov965x_write_array(client,
 555                                                  ov965x_init_regs);
 556                         ov965x->apply_frame_fmt = 1;
 557                         ov965x->ctrls.update = 1;
 558                 }
 559         }
 560         if (!ret)
 561                 ov965x->power += on ? 1 : -1;
 562
 563         WARN_ON(ov965x->power < 0);
 564         mutex_unlock(&ov965x->lock);
 565         return ret;
 566 }
 567
 568 /*
 569  * V4L2 controls
 570  */
 571
 572 static void ov965x_update_exposure_ctrl(struct ov965x *ov965x)
 573 {
 574         struct v4l2_ctrl *ctrl = ov965x->ctrls.exposure;
 575         unsigned long fint, trow;
 576         int min, max, def;
 577         u8 clkrc;
 578
 579         mutex_lock(&ov965x->lock);
 580         if (WARN_ON(!ctrl || !ov965x->frame_size)) {
 581                 mutex_unlock(&ov965x->lock);
 582                 return;
 583         }
 584         clkrc = DEF_CLKRC + ov965x->fiv->clkrc_div;
 585         /* Calculate internal clock frequency */
 586         fint = ov965x->mclk_frequency * ((clkrc >> 7) + 1) /
 587                                 ((2 * ((clkrc & 0x3f) + 1)));
 588         /* and the row interval (in us). */
 589         trow = (2 * 1520 * 1000000UL) / fint;
 590         max = ov965x->frame_size->max_exp_lines * trow;
 591         ov965x->exp_row_interval = trow;
 592         mutex_unlock(&ov965x->lock);
 593
 594         v4l2_dbg(1, debug, &ov965x->sd, "clkrc: %#x, fi: %lu, tr: %lu, %d\n",
 595                  clkrc, fint, trow, max);
 596
 597         /* Update exposure time range to match current frame format. */
 598         min = (trow + 100) / 100;
 599         max = (max - 100) / 100;
 600         def = min + (max - min) / 2;
 601
 602         if (v4l2_ctrl_modify_range(ctrl, min, max, 1, def))
 603                 v4l2_err(&ov965x->sd, "Exposure ctrl range update failed\n");
 604 }
 605
 606 static int ov965x_set_banding_filter(struct ov965x *ov965x, int value)
 607 {
 608         unsigned long mbd, light_freq;
 609         int ret;
 610         u8 reg;
 611
 612         ret = ov965x_read(ov965x->client, REG_COM8, &reg);
 613         if (!ret) {
 614                 if (value == V4L2_CID_POWER_LINE_FREQUENCY_DISABLED)
 615                         reg &= ~COM8_BFILT;
 616                 else
 617                         reg |= COM8_BFILT;
 618                 ret = ov965x_write(ov965x->client, REG_COM8, reg);
 619         }
 620         if (value == V4L2_CID_POWER_LINE_FREQUENCY_DISABLED)
 621                 return 0;
 622         if (WARN_ON(!ov965x->fiv))
 623                 return -EINVAL;
 624         /* Set minimal exposure time for 50/60 HZ lighting */
 625         if (value == V4L2_CID_POWER_LINE_FREQUENCY_50HZ)
 626                 light_freq = 50;
 627         else
 628                 light_freq = 60;
 629         mbd = (1000UL * ov965x->fiv->interval.denominator *
 630                ov965x->frame_size->max_exp_lines) /
 631                ov965x->fiv->interval.numerator;
 632         mbd = ((mbd / (light_freq * 2)) + 500) / 1000UL;
 633
 634         return ov965x_write(ov965x->client, REG_MBD, mbd);
 635 }
 636
 637 static int ov965x_set_white_balance(struct ov965x *ov965x, int awb)
 638 {
 639         int ret;
 640         u8 reg;
 641
 642         ret = ov965x_read(ov965x->client, REG_COM8, &reg);
 643         if (!ret) {
 644                 reg = awb ? reg | REG_COM8 : reg & ~REG_COM8;
 645                 ret = ov965x_write(ov965x->client, REG_COM8, reg);
 646         }
 647         if (!ret && !awb) {
 648                 ret = ov965x_write(ov965x->client, REG_BLUE,
 649                                    ov965x->ctrls.blue_balance->val);
 650                 if (ret < 0)
 651                         return ret;
 652                 ret = ov965x_write(ov965x->client, REG_RED,
 653                                    ov965x->ctrls.red_balance->val);
 654         }
 655         return ret;
 656 }
 657
 658 #define NUM_BR_LEVELS   7
 659 #define NUM_BR_REGS     3
 660
 661 static int ov965x_set_brightness(struct ov965x *ov965x, int val)
 662 {
 663         static const u8 regs[NUM_BR_LEVELS + 1][NUM_BR_REGS] = {
 664                 { REG_AEW, REG_AEB, REG_VPT },
 665                 { 0x1c, 0x12, 0x50 }, /* -3 */
 666                 { 0x3d, 0x30, 0x71 }, /* -2 */
 667                 { 0x50, 0x44, 0x92 }, /* -1 */
 668                 { 0x70, 0x64, 0xc3 }, /*  0 */
 669                 { 0x90, 0x84, 0xd4 }, /* +1 */
 670                 { 0xc4, 0xbf, 0xf9 }, /* +2 */
 671                 { 0xd8, 0xd0, 0xfa }, /* +3 */
 672         };
 673         int i, ret = 0;
 674
 675         val += (NUM_BR_LEVELS / 2 + 1);
 676         if (val > NUM_BR_LEVELS)
 677                 return -EINVAL;
 678
 679         for (i = 0; i < NUM_BR_REGS && !ret; i++)
 680                 ret = ov965x_write(ov965x->client, regs[0][i],
 681                                    regs[val][i]);
 682         return ret;
 683 }
 684
 685 static int ov965x_set_gain(struct ov965x *ov965x, int auto_gain)
 686 {
 687         struct i2c_client *client = ov965x->client;
 688         struct ov965x_ctrls *ctrls = &ov965x->ctrls;
 689         int ret = 0;
 690         u8 reg;
 691         /*
 692          * For manual mode we need to disable AGC first, so
 693          * gain value in REG_VREF, REG_GAIN is not overwritten.
 694          */
 695         if (ctrls->auto_gain->is_new) {
 696                 ret = ov965x_read(client, REG_COM8, &reg);
 697                 if (ret < 0)
 698                         return ret;
 699                 if (ctrls->auto_gain->val)
 700                         reg |= COM8_AGC;
 701                 else
 702                         reg &= ~COM8_AGC;
 703                 ret = ov965x_write(client, REG_COM8, reg);
 704                 if (ret < 0)
 705                         return ret;
 706         }
 707
 708         if (ctrls->gain->is_new && !auto_gain) {
 709                 unsigned int gain = ctrls->gain->val;
 710                 unsigned int rgain;
 711                 int m;
 712                 /*
 713                  * Convert gain control value to the sensor's gain
 714                  * registers (VREF[7:6], GAIN[7:0]) format.
 715                  */
 716                 for (m = 6; m >= 0; m--)
 717                         if (gain >= (1 << m) * 16)
 718                                 break;
 719
 720                 /* Sanity check: don't adjust the gain with a negative value */
 721                 if (m < 0)
 722                         return -EINVAL;
 723
 724                 rgain = (gain - ((1 << m) * 16)) / (1 << m);
 725                 rgain |= (((1 << m) - 1) << 4);
 726
 727                 ret = ov965x_write(client, REG_GAIN, rgain & 0xff);
 728                 if (ret < 0)
 729                         return ret;
 730                 ret = ov965x_read(client, REG_VREF, &reg);
 731                 if (ret < 0)
 732                         return ret;
 733                 reg &= ~VREF_GAIN_MASK;
 734                 reg |= (((rgain >> 8) & 0x3) << 6);
 735                 ret = ov965x_write(client, REG_VREF, reg);
 736                 if (ret < 0)
 737                         return ret;
 738                 /* Return updated control's value to userspace */
 739                 ctrls->gain->val = (1 << m) * (16 + (rgain & 0xf));
 740         }
 741
 742         return ret;
 743 }
 744
 745 static int ov965x_set_sharpness(struct ov965x *ov965x, unsigned int value)
 746 {
 747         u8 com14, edge;
 748         int ret;
 749
 750         ret = ov965x_read(ov965x->client, REG_COM14, &com14);
 751         if (ret < 0)
 752                 return ret;
 753         ret = ov965x_read(ov965x->client, REG_EDGE, &edge);
 754         if (ret < 0)
 755                 return ret;
 756         com14 = value ? com14 | COM14_EDGE_EN : com14 & ~COM14_EDGE_EN;
 757         value--;
 758         if (value > 0x0f) {
 759                 com14 |= COM14_EEF_X2;
 760                 value >>= 1;
 761         } else {
 762                 com14 &= ~COM14_EEF_X2;
 763         }
 764         ret = ov965x_write(ov965x->client, REG_COM14, com14);
 765         if (ret < 0)
 766                 return ret;
 767
 768         edge &= ~EDGE_FACTOR_MASK;
 769         edge |= ((u8)value & 0x0f);
 770
 771         return ov965x_write(ov965x->client, REG_EDGE, edge);
 772 }
 773
 774 static int ov965x_set_exposure(struct ov965x *ov965x, int exp)
 775 {
 776         struct i2c_client *client = ov965x->client;
 777         struct ov965x_ctrls *ctrls = &ov965x->ctrls;
 778         bool auto_exposure = (exp == V4L2_EXPOSURE_AUTO);
 779         int ret;
 780         u8 reg;
 781
 782         if (ctrls->auto_exp->is_new) {
 783                 ret = ov965x_read(client, REG_COM8, &reg);
 784                 if (ret < 0)
 785                         return ret;
 786                 if (auto_exposure)
 787                         reg |= (COM8_AEC | COM8_AGC);
 788                 else
 789                         reg &= ~(COM8_AEC | COM8_AGC);
 790                 ret = ov965x_write(client, REG_COM8, reg);
 791                 if (ret < 0)
 792                         return ret;
 793         }
 794
 795         if (!auto_exposure && ctrls->exposure->is_new) {
 796                 unsigned int exposure = (ctrls->exposure->val * 100)
 797                                          / ov965x->exp_row_interval;
 798                 /*
 799                  * Manual exposure value
 800                  * [b15:b0] - AECHM (b15:b10), AECH (b9:b2), COM1 (b1:b0)
 801                  */
 802                 ret = ov965x_write(client, REG_COM1, exposure & 0x3);
 803                 if (!ret)
 804                         ret = ov965x_write(client, REG_AECH,
 805                                            (exposure >> 2) & 0xff);
 806                 if (!ret)
 807                         ret = ov965x_write(client, REG_AECHM,
 808                                            (exposure >> 10) & 0x3f);
 809                 /* Update the value to minimize rounding errors */
 810                 ctrls->exposure->val = ((exposure * ov965x->exp_row_interval)
 811                                                         + 50) / 100;
 812                 if (ret < 0)
 813                         return ret;
 814         }
 815
 816         v4l2_ctrl_activate(ov965x->ctrls.brightness, !exp);
 817         return 0;
 818 }
 819
 820 static int ov965x_set_flip(struct ov965x *ov965x)
 821 {
 822         u8 mvfp = 0;
 823
 824         if (ov965x->ctrls.hflip->val)
 825                 mvfp |= MVFP_MIRROR;
 826
 827         if (ov965x->ctrls.vflip->val)
 828                 mvfp |= MVFP_FLIP;
 829
 830         return ov965x_write(ov965x->client, REG_MVFP, mvfp);
 831 }
 832
 833 #define NUM_SAT_LEVELS  5
 834 #define NUM_SAT_REGS    6
 835
 836 static int ov965x_set_saturation(struct ov965x *ov965x, int val)
 837 {
 838         static const u8 regs[NUM_SAT_LEVELS][NUM_SAT_REGS] = {
 839                 /* MTX(1)...MTX(6) */
 840                 { 0x1d, 0x1f, 0x02, 0x09, 0x13, 0x1c }, /* -2 */
 841                 { 0x2e, 0x31, 0x02, 0x0e, 0x1e, 0x2d }, /* -1 */
 842                 { 0x3a, 0x3d, 0x03, 0x12, 0x26, 0x38 }, /*  0 */
 843                 { 0x46, 0x49, 0x04, 0x16, 0x2e, 0x43 }, /* +1 */
 844                 { 0x57, 0x5c, 0x05, 0x1b, 0x39, 0x54 }, /* +2 */
 845         };
 846         u8 addr = REG_MTX(1);
 847         int i, ret = 0;
 848
 849         val += (NUM_SAT_LEVELS / 2);
 850         if (val >= NUM_SAT_LEVELS)
 851                 return -EINVAL;
 852
 853         for (i = 0; i < NUM_SAT_REGS && !ret; i++)
 854                 ret = ov965x_write(ov965x->client, addr + i, regs[val][i]);
 855
 856         return ret;
 857 }
 858
 859 static int ov965x_set_test_pattern(struct ov965x *ov965x, int value)
 860 {
 861         int ret;
 862         u8 reg;
 863
 864         ret = ov965x_read(ov965x->client, REG_COM23, &reg);
 865         if (ret < 0)
 866                 return ret;
 867         reg = value ? reg | COM23_TEST_MODE : reg & ~COM23_TEST_MODE;
 868         return ov965x_write(ov965x->client, REG_COM23, reg);
 869 }
 870
 871 static int __g_volatile_ctrl(struct ov965x *ov965x, struct v4l2_ctrl *ctrl)
 872 {
 873         struct i2c_client *client = ov965x->client;
 874         unsigned int exposure, gain, m;
 875         u8 reg0, reg1, reg2;
 876         int ret;
 877
 878         if (!ov965x->power)
 879                 return 0;
 880
 881         switch (ctrl->id) {
 882         case V4L2_CID_AUTOGAIN:
 883                 if (!ctrl->val)
 884                         return 0;
 885                 ret = ov965x_read(client, REG_GAIN, &reg0);
 886                 if (ret < 0)
 887                         return ret;
 888                 ret = ov965x_read(client, REG_VREF, &reg1);
 889                 if (ret < 0)
 890                         return ret;
 891                 gain = ((reg1 >> 6) << 8) | reg0;
 892                 m = 0x01 << fls(gain >> 4);
 893                 ov965x->ctrls.gain->val = m * (16 + (gain & 0xf));
 894                 break;
 895
 896         case V4L2_CID_EXPOSURE_AUTO:
 897                 if (ctrl->val == V4L2_EXPOSURE_MANUAL)
 898                         return 0;
 899                 ret = ov965x_read(client, REG_COM1, &reg0);
 900                 if (ret < 0)
 901                         return ret;
 902                 ret = ov965x_read(client, REG_AECH, &reg1);
 903                 if (ret < 0)
 904                         return ret;
 905                 ret = ov965x_read(client, REG_AECHM, &reg2);
 906                 if (ret < 0)
 907                         return ret;
 908                 exposure = ((reg2 & 0x3f) << 10) | (reg1 << 2) |
 909                                                 (reg0 & 0x3);
 910                 ov965x->ctrls.exposure->val = ((exposure *
 911                                 ov965x->exp_row_interval) + 50) / 100;
 912                 break;
 913         }
 914
 915         return 0;
 916 }
 917
 918 static int ov965x_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 919 {
 920         struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
 921         struct ov965x *ov965x = to_ov965x(sd);
 922         int ret;
 923
 924         v4l2_dbg(1, debug, sd, "g_ctrl: %s\n", ctrl->name);
 925
 926         mutex_lock(&ov965x->lock);
 927         ret = __g_volatile_ctrl(ov965x, ctrl);
 928         mutex_unlock(&ov965x->lock);
 929         return ret;
 930 }
 931
 932 static int ov965x_s_ctrl(struct v4l2_ctrl *ctrl)
 933 {
 934         struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
 935         struct ov965x *ov965x = to_ov965x(sd);
 936         int ret = -EINVAL;
 937
 938         v4l2_dbg(1, debug, sd, "s_ctrl: %s, value: %d. power: %d\n",
 939                  ctrl->name, ctrl->val, ov965x->power);
 940
 941         mutex_lock(&ov965x->lock);
 942         /*
 943          * If the device is not powered up now postpone applying control's
 944          * value to the hardware, until it is ready to accept commands.
 945          */
 946         if (ov965x->power == 0) {
 947                 mutex_unlock(&ov965x->lock);
 948                 return 0;
 949         }
 950
 951         switch (ctrl->id) {
 952         case V4L2_CID_AUTO_WHITE_BALANCE:
 953                 ret = ov965x_set_white_balance(ov965x, ctrl->val);
 954                 break;
 955
 956         case V4L2_CID_BRIGHTNESS:
 957                 ret = ov965x_set_brightness(ov965x, ctrl->val);
 958                 break;
 959
 960         case V4L2_CID_EXPOSURE_AUTO:
 961                 ret = ov965x_set_exposure(ov965x, ctrl->val);
 962                 break;
 963
 964         case V4L2_CID_AUTOGAIN:
 965                 ret = ov965x_set_gain(ov965x, ctrl->val);
 966                 break;
 967
 968         case V4L2_CID_HFLIP:
 969                 ret = ov965x_set_flip(ov965x);
 970                 break;
 971
 972         case V4L2_CID_POWER_LINE_FREQUENCY:
 973                 ret = ov965x_set_banding_filter(ov965x, ctrl->val);
 974                 break;
 975
 976         case V4L2_CID_SATURATION:
 977                 ret = ov965x_set_saturation(ov965x, ctrl->val);
 978                 break;
 979
 980         case V4L2_CID_SHARPNESS:
 981                 ret = ov965x_set_sharpness(ov965x, ctrl->val);
 982                 break;
 983
 984         case V4L2_CID_TEST_PATTERN:
 985                 ret = ov965x_set_test_pattern(ov965x, ctrl->val);
 986                 break;
 987         }
 988
 989         mutex_unlock(&ov965x->lock);
 990         return ret;
 991 }
 992
 993 static const struct v4l2_ctrl_ops ov965x_ctrl_ops = {
 994         .g_volatile_ctrl = ov965x_g_volatile_ctrl,
 995         .s_ctrl = ov965x_s_ctrl,
 996 };
 997
 998 static const char * const test_pattern_menu[] = {
 999         "Disabled",
1000         "Color bars",
1001 };
1002
1003 static int ov965x_initialize_controls(struct ov965x *ov965x)
1004 {
1005         const struct v4l2_ctrl_ops *ops = &ov965x_ctrl_ops;
1006         struct ov965x_ctrls *ctrls = &ov965x->ctrls;
1007         struct v4l2_ctrl_handler *hdl = &ctrls->handler;
1008         int ret;
1009
1010         ret = v4l2_ctrl_handler_init(hdl, 16);
1011         if (ret < 0)
1012                 return ret;
1013
1014         /* Auto/manual white balance */
1015         ctrls->auto_wb = v4l2_ctrl_new_std(hdl, ops,
1016                                            V4L2_CID_AUTO_WHITE_BALANCE,
1017                                            0, 1, 1, 1);
1018         ctrls->blue_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BLUE_BALANCE,
1019                                                 0, 0xff, 1, 0x80);
1020         ctrls->red_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_RED_BALANCE,
1021                                                0, 0xff, 1, 0x80);
1022         /* Auto/manual exposure */
1023         ctrls->auto_exp =
1024                 v4l2_ctrl_new_std_menu(hdl, ops,
1025                                        V4L2_CID_EXPOSURE_AUTO,
1026                                        V4L2_EXPOSURE_MANUAL, 0,
1027                                        V4L2_EXPOSURE_AUTO);
1028         /* Exposure time, in 100 us units. min/max is updated dynamically. */
1029         ctrls->exposure = v4l2_ctrl_new_std(hdl, ops,
1030                                             V4L2_CID_EXPOSURE_ABSOLUTE,
1031                                             2, 1500, 1, 500);
1032         /* Auto/manual gain */
1033         ctrls->auto_gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTOGAIN,
1034                                              0, 1, 1, 1);
1035         ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN,
1036                                         16, 64 * (16 + 15), 1, 64 * 16);
1037
1038         ctrls->saturation = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SATURATION,
1039                                               -2, 2, 1, 0);
1040         ctrls->brightness = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BRIGHTNESS,
1041                                               -3, 3, 1, 0);
1042         ctrls->sharpness = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SHARPNESS,
1043                                              0, 32, 1, 6);
1044
1045         ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
1046         ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
1047
1048         ctrls->light_freq =
1049                 v4l2_ctrl_new_std_menu(hdl, ops,
1050                                        V4L2_CID_POWER_LINE_FREQUENCY,
1051                                        V4L2_CID_POWER_LINE_FREQUENCY_60HZ, ~0x7,
1052                                        V4L2_CID_POWER_LINE_FREQUENCY_50HZ);
1053
1054         v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
1055                                      ARRAY_SIZE(test_pattern_menu) - 1, 0, 0,
1056                                      test_pattern_menu);
1057         if (hdl->error) {
1058                 ret = hdl->error;
1059                 v4l2_ctrl_handler_free(hdl);
1060                 return ret;
1061         }
1062
1063         ctrls->gain->flags |= V4L2_CTRL_FLAG_VOLATILE;
1064         ctrls->exposure->flags |= V4L2_CTRL_FLAG_VOLATILE;
1065
1066         v4l2_ctrl_auto_cluster(3, &ctrls->auto_wb, 0, false);
1067         v4l2_ctrl_auto_cluster(2, &ctrls->auto_gain, 0, true);
1068         v4l2_ctrl_auto_cluster(2, &ctrls->auto_exp, 1, true);
1069         v4l2_ctrl_cluster(2, &ctrls->hflip);
1070
1071         ov965x->sd.ctrl_handler = hdl;
1072         return 0;
1073 }
1074
1075 /*
1076  * V4L2 subdev video and pad level operations
1077  */
1078 static void ov965x_get_default_format(struct v4l2_mbus_framefmt *mf)
1079 {
1080         mf->width = ov965x_framesizes[0].width;
1081         mf->height = ov965x_framesizes[0].height;
1082         mf->colorspace = ov965x_formats[0].colorspace;
1083         mf->code = ov965x_formats[0].code;
1084         mf->field = V4L2_FIELD_NONE;
1085 }
1086
1087 static int ov965x_enum_mbus_code(struct v4l2_subdev *sd,
1088                                  struct v4l2_subdev_pad_config *cfg,
1089                                  struct v4l2_subdev_mbus_code_enum *code)
1090 {
1091         if (code->index >= ARRAY_SIZE(ov965x_formats))
1092                 return -EINVAL;
1093
1094         code->code = ov965x_formats[code->index].code;
1095         return 0;
1096 }
1097
1098 static int ov965x_enum_frame_sizes(struct v4l2_subdev *sd,
1099                                    struct v4l2_subdev_pad_config *cfg,
1100                                    struct v4l2_subdev_frame_size_enum *fse)
1101 {
1102         int i = ARRAY_SIZE(ov965x_formats);
1103
1104         if (fse->index >= ARRAY_SIZE(ov965x_framesizes))
1105                 return -EINVAL;
1106
1107         while (--i)
1108                 if (fse->code == ov965x_formats[i].code)
1109                         break;
1110
1111         fse->code = ov965x_formats[i].code;
1112
1113         fse->min_width  = ov965x_framesizes[fse->index].width;
1114         fse->max_width  = fse->min_width;
1115         fse->max_height = ov965x_framesizes[fse->index].height;
1116         fse->min_height = fse->max_height;
1117
1118         return 0;
1119 }
1120
1121 static int ov965x_g_frame_interval(struct v4l2_subdev *sd,
1122                                    struct v4l2_subdev_frame_interval *fi)
1123 {
1124         struct ov965x *ov965x = to_ov965x(sd);
1125
1126         mutex_lock(&ov965x->lock);
1127         fi->interval = ov965x->fiv->interval;
1128         mutex_unlock(&ov965x->lock);
1129
1130         return 0;
1131 }
1132
1133 static int __ov965x_set_frame_interval(struct ov965x *ov965x,
1134                                        struct v4l2_subdev_frame_interval *fi)
1135 {
1136         struct v4l2_mbus_framefmt *mbus_fmt = &ov965x->format;
1137         const struct ov965x_interval *fiv = &ov965x_intervals[0];
1138         u64 req_int, err, min_err = ~0ULL;
1139         unsigned int i;
1140
1141         if (fi->interval.denominator == 0)
1142                 return -EINVAL;
1143
1144         req_int = (u64)fi->interval.numerator * 10000;
1145         do_div(req_int, fi->interval.denominator);
1146
1147         for (i = 0; i < ARRAY_SIZE(ov965x_intervals); i++) {
1148                 const struct ov965x_interval *iv = &ov965x_intervals[i];
1149
1150                 if (mbus_fmt->width != iv->size.width ||
1151                     mbus_fmt->height != iv->size.height)
1152                         continue;
1153                 err = abs((u64)(iv->interval.numerator * 10000) /
1154                             iv->interval.denominator - req_int);
1155                 if (err < min_err) {
1156                         fiv = iv;
1157                         min_err = err;
1158                 }
1159         }
1160         ov965x->fiv = fiv;
1161
1162         v4l2_dbg(1, debug, &ov965x->sd, "Changed frame interval to %u us\n",
1163                  fiv->interval.numerator * 1000000 / fiv->interval.denominator);
1164
1165         return 0;
1166 }
1167
1168 static int ov965x_s_frame_interval(struct v4l2_subdev *sd,
1169                                    struct v4l2_subdev_frame_interval *fi)
1170 {
1171         struct ov965x *ov965x = to_ov965x(sd);
1172         int ret;
1173
1174         v4l2_dbg(1, debug, sd, "Setting %d/%d frame interval\n",
1175                  fi->interval.numerator, fi->interval.denominator);
1176
1177         mutex_lock(&ov965x->lock);
1178         ret = __ov965x_set_frame_interval(ov965x, fi);
1179         ov965x->apply_frame_fmt = 1;
1180         mutex_unlock(&ov965x->lock);
1181         return ret;
1182 }
1183
1184 static int ov965x_get_fmt(struct v4l2_subdev *sd,
1185                           struct v4l2_subdev_pad_config *cfg,
1186                           struct v4l2_subdev_format *fmt)
1187 {
1188         struct ov965x *ov965x = to_ov965x(sd);
1189         struct v4l2_mbus_framefmt *mf;
1190
1191         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1192                 mf = v4l2_subdev_get_try_format(sd, cfg, 0);
1193                 fmt->format = *mf;
1194                 return 0;
1195         }
1196
1197         mutex_lock(&ov965x->lock);
1198         fmt->format = ov965x->format;
1199         mutex_unlock(&ov965x->lock);
1200
1201         return 0;
1202 }
1203
1204 static void __ov965x_try_frame_size(struct v4l2_mbus_framefmt *mf,
1205                                     const struct ov965x_framesize **size)
1206 {
1207         const struct ov965x_framesize *fsize = &ov965x_framesizes[0],
1208                 *match = NULL;
1209         int i = ARRAY_SIZE(ov965x_framesizes);
1210         unsigned int min_err = UINT_MAX;
1211
1212         while (i--) {
1213                 int err = abs(fsize->width - mf->width)
1214                                 + abs(fsize->height - mf->height);
1215                 if (err < min_err) {
1216                         min_err = err;
1217                         match = fsize;
1218                 }
1219                 fsize++;
1220         }
1221         if (!match)
1222                 match = &ov965x_framesizes[0];
1223         mf->width  = match->width;
1224         mf->height = match->height;
1225         if (size)
1226                 *size = match;
1227 }
1228
1229 static int ov965x_set_fmt(struct v4l2_subdev *sd,
1230                           struct v4l2_subdev_pad_config *cfg,
1231                           struct v4l2_subdev_format *fmt)
1232 {
1233         unsigned int index = ARRAY_SIZE(ov965x_formats);
1234         struct v4l2_mbus_framefmt *mf = &fmt->format;
1235         struct ov965x *ov965x = to_ov965x(sd);
1236         const struct ov965x_framesize *size = NULL;
1237         int ret = 0;
1238
1239         __ov965x_try_frame_size(mf, &size);
1240
1241         while (--index)
1242                 if (ov965x_formats[index].code == mf->code)
1243                         break;
1244
1245         mf->colorspace  = V4L2_COLORSPACE_JPEG;
1246         mf->code        = ov965x_formats[index].code;
1247         mf->field       = V4L2_FIELD_NONE;
1248
1249         mutex_lock(&ov965x->lock);
1250
1251         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1252                 if (cfg) {
1253                         mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
1254                         *mf = fmt->format;
1255                 }
1256         } else {
1257                 if (ov965x->streaming) {
1258                         ret = -EBUSY;
1259                 } else {
1260                         ov965x->frame_size = size;
1261                         ov965x->format = fmt->format;
1262                         ov965x->tslb_reg = ov965x_formats[index].tslb_reg;
1263                         ov965x->apply_frame_fmt = 1;
1264                 }
1265         }
1266
1267         if (!ret && fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
1268                 struct v4l2_subdev_frame_interval fiv = {
1269                         .interval = { 0, 1 }
1270                 };
1271                 /* Reset to minimum possible frame interval */
1272                 __ov965x_set_frame_interval(ov965x, &fiv);
1273         }
1274         mutex_unlock(&ov965x->lock);
1275
1276         if (!ret)
1277                 ov965x_update_exposure_ctrl(ov965x);
1278
1279         return ret;
1280 }
1281
1282 static int ov965x_set_frame_size(struct ov965x *ov965x)
1283 {
1284         int i, ret = 0;
1285
1286         for (i = 0; ret == 0 && i < NUM_FMT_REGS; i++)
1287                 ret = ov965x_write(ov965x->client, frame_size_reg_addr[i],
1288                                    ov965x->frame_size->regs[i]);
1289         return ret;
1290 }
1291
1292 static int __ov965x_set_params(struct ov965x *ov965x)
1293 {
1294         struct i2c_client *client = ov965x->client;
1295         struct ov965x_ctrls *ctrls = &ov965x->ctrls;
1296         int ret = 0;
1297         u8 reg;
1298
1299         if (ov965x->apply_frame_fmt) {
1300                 reg = DEF_CLKRC + ov965x->fiv->clkrc_div;
1301                 ret = ov965x_write(client, REG_CLKRC, reg);
1302                 if (ret < 0)
1303                         return ret;
1304                 ret = ov965x_set_frame_size(ov965x);
1305                 if (ret < 0)
1306                         return ret;
1307                 ret = ov965x_read(client, REG_TSLB, &reg);
1308                 if (ret < 0)
1309                         return ret;
1310                 reg &= ~TSLB_YUYV_MASK;
1311                 reg |= ov965x->tslb_reg;
1312                 ret = ov965x_write(client, REG_TSLB, reg);
1313                 if (ret < 0)
1314                         return ret;
1315         }
1316         ret = ov965x_set_default_gamma_curve(ov965x);
1317         if (ret < 0)
1318                 return ret;
1319         ret = ov965x_set_color_matrix(ov965x);
1320         if (ret < 0)
1321                 return ret;
1322         /*
1323          * Select manual banding filter, the filter will
1324          * be enabled further if required.
1325          */
1326         ret = ov965x_read(client, REG_COM11, &reg);
1327         if (!ret)
1328                 reg |= COM11_BANDING;
1329         ret = ov965x_write(client, REG_COM11, reg);
1330         if (ret < 0)
1331                 return ret;
1332         /*
1333          * Banding filter (REG_MBD value) needs to match selected
1334          * resolution and frame rate, so it's always updated here.
1335          */
1336         return ov965x_set_banding_filter(ov965x, ctrls->light_freq->val);
1337 }
1338
1339 static int ov965x_s_stream(struct v4l2_subdev *sd, int on)
1340 {
1341         struct i2c_client *client = v4l2_get_subdevdata(sd);
1342         struct ov965x *ov965x = to_ov965x(sd);
1343         struct ov965x_ctrls *ctrls = &ov965x->ctrls;
1344         int ret = 0;
1345
1346         v4l2_dbg(1, debug, client, "%s: on: %d\n", __func__, on);
1347
1348         mutex_lock(&ov965x->lock);
1349         if (ov965x->streaming == !on) {
1350                 if (on)
1351                         ret = __ov965x_set_params(ov965x);
1352
1353                 if (!ret && ctrls->update) {
1354                         /*
1355                          * ov965x_s_ctrl callback takes the mutex
1356                          * so it needs to be released here.
1357                          */
1358                         mutex_unlock(&ov965x->lock);
1359                         ret = v4l2_ctrl_handler_setup(&ctrls->handler);
1360
1361                         mutex_lock(&ov965x->lock);
1362                         if (!ret)
1363                                 ctrls->update = 0;
1364                 }
1365                 if (!ret)
1366                         ret = ov965x_write(client, REG_COM2,
1367                                            on ? 0x01 : 0x11);
1368         }
1369         if (!ret)
1370                 ov965x->streaming += on ? 1 : -1;
1371
1372         WARN_ON(ov965x->streaming < 0);
1373         mutex_unlock(&ov965x->lock);
1374
1375         return ret;
1376 }
1377
1378 /*
1379  * V4L2 subdev internal operations
1380  */
1381 static int ov965x_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1382 {
1383         struct v4l2_mbus_framefmt *mf =
1384                 v4l2_subdev_get_try_format(sd, fh->pad, 0);
1385
1386         ov965x_get_default_format(mf);
1387         return 0;
1388 }
1389
1390 static const struct v4l2_subdev_pad_ops ov965x_pad_ops = {
1391         .enum_mbus_code = ov965x_enum_mbus_code,
1392         .enum_frame_size = ov965x_enum_frame_sizes,
1393         .get_fmt = ov965x_get_fmt,
1394         .set_fmt = ov965x_set_fmt,
1395 };
1396
1397 static const struct v4l2_subdev_video_ops ov965x_video_ops = {
1398         .s_stream = ov965x_s_stream,
1399         .g_frame_interval = ov965x_g_frame_interval,
1400         .s_frame_interval = ov965x_s_frame_interval,
1401
1402 };
1403
1404 static const struct v4l2_subdev_internal_ops ov965x_sd_internal_ops = {
1405         .open = ov965x_open,
1406 };
1407
1408 static const struct v4l2_subdev_core_ops ov965x_core_ops = {
1409         .s_power = ov965x_s_power,
1410         .log_status = v4l2_ctrl_subdev_log_status,
1411         .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
1412         .unsubscribe_event = v4l2_event_subdev_unsubscribe,
1413 };
1414
1415 static const struct v4l2_subdev_ops ov965x_subdev_ops = {
1416         .core = &ov965x_core_ops,
1417         .pad = &ov965x_pad_ops,
1418         .video = &ov965x_video_ops,
1419 };
1420
1421 /*
1422  * Reset and power down GPIOs configuration
1423  */
1424 static int ov965x_configure_gpios_pdata(struct ov965x *ov965x,
1425                                 const struct ov9650_platform_data *pdata)
1426 {
1427         int ret, i;
1428         int gpios[NUM_GPIOS];
1429
1430         gpios[GPIO_PWDN] = pdata->gpio_pwdn;
1431         gpios[GPIO_RST]  = pdata->gpio_reset;
1432
1433         for (i = 0; i < ARRAY_SIZE(ov965x->gpios); i++) {
1434                 int gpio = gpios[i];
1435
1436                 if (!gpio_is_valid(gpio))
1437                         continue;
1438                 ret = devm_gpio_request_one(&ov965x->client->dev, gpio,
1439                                             GPIOF_OUT_INIT_HIGH, "OV965X");
1440                 if (ret < 0)
1441                         return ret;
1442                 v4l2_dbg(1, debug, &ov965x->sd, "set gpio %d to 1\n", gpio);
1443
1444                 gpio_set_value_cansleep(gpio, 1);
1445                 gpio_export(gpio, 0);
1446                 ov965x->gpios[i] = gpio_to_desc(gpio);
1447         }
1448
1449         return 0;
1450 }
1451
1452 static int ov965x_configure_gpios(struct ov965x *ov965x)
1453 {
1454         struct device *dev = &ov965x->client->dev;
1455
1456         ov965x->gpios[GPIO_PWDN] = devm_gpiod_get_optional(dev, "powerdown",
1457                                                         GPIOD_OUT_HIGH);
1458         if (IS_ERR(ov965x->gpios[GPIO_PWDN])) {
1459                 dev_info(dev, "can't get %s GPIO\n", "powerdown");
1460                 return PTR_ERR(ov965x->gpios[GPIO_PWDN]);
1461         }
1462
1463         ov965x->gpios[GPIO_RST] = devm_gpiod_get_optional(dev, "reset",
1464                                                         GPIOD_OUT_HIGH);
1465         if (IS_ERR(ov965x->gpios[GPIO_RST])) {
1466                 dev_info(dev, "can't get %s GPIO\n", "reset");
1467                 return PTR_ERR(ov965x->gpios[GPIO_RST]);
1468         }
1469
1470         return 0;
1471 }
1472
1473 static int ov965x_detect_sensor(struct v4l2_subdev *sd)
1474 {
1475         struct i2c_client *client = v4l2_get_subdevdata(sd);
1476         struct ov965x *ov965x = to_ov965x(sd);
1477         u8 pid, ver;
1478         int ret;
1479
1480         mutex_lock(&ov965x->lock);
1481         ret = __ov965x_set_power(ov965x, 1);
1482         if (ret)
1483                 goto out;
1484
1485         msleep(25);
1486
1487         /* Check sensor revision */
1488         ret = ov965x_read(client, REG_PID, &pid);
1489         if (!ret)
1490                 ret = ov965x_read(client, REG_VER, &ver);
1491
1492         __ov965x_set_power(ov965x, 0);
1493
1494         if (!ret) {
1495                 ov965x->id = OV965X_ID(pid, ver);
1496                 if (ov965x->id == OV9650_ID || ov965x->id == OV9652_ID) {
1497                         v4l2_info(sd, "Found OV%04X sensor\n", ov965x->id);
1498                 } else {
1499                         v4l2_err(sd, "Sensor detection failed (%04X, %d)\n",
1500                                  ov965x->id, ret);
1501                         ret = -ENODEV;
1502                 }
1503         }
1504 out:
1505         mutex_unlock(&ov965x->lock);
1506
1507         return ret;
1508 }
1509
1510 static int ov965x_probe(struct i2c_client *client,
1511                         const struct i2c_device_id *id)
1512 {
1513         const struct ov9650_platform_data *pdata = client->dev.platform_data;
1514         struct v4l2_subdev *sd;
1515         struct ov965x *ov965x;
1516         int ret;
1517
1518         ov965x = devm_kzalloc(&client->dev, sizeof(*ov965x), GFP_KERNEL);
1519         if (!ov965x)
1520                 return -ENOMEM;
1521
1522         ov965x->client = client;
1523
1524         if (pdata) {
1525                 if (pdata->mclk_frequency == 0) {
1526                         dev_err(&client->dev, "MCLK frequency not specified\n");
1527                         return -EINVAL;
1528                 }
1529                 ov965x->mclk_frequency = pdata->mclk_frequency;
1530
1531                 ret = ov965x_configure_gpios_pdata(ov965x, pdata);
1532                 if (ret < 0)
1533                         return ret;
1534         } else if (dev_fwnode(&client->dev)) {
1535                 ov965x->clk = devm_clk_get(&ov965x->client->dev, NULL);
1536                 if (IS_ERR(ov965x->clk))
1537                         return PTR_ERR(ov965x->clk);
1538                 ov965x->mclk_frequency = clk_get_rate(ov965x->clk);
1539
1540                 ret = ov965x_configure_gpios(ov965x);
1541                 if (ret < 0)
1542                         return ret;
1543         } else {
1544                 dev_err(&client->dev,
1545                         "Neither platform data nor device property specified\n");
1546
1547                 return -EINVAL;
1548         }
1549
1550         mutex_init(&ov965x->lock);
1551
1552         sd = &ov965x->sd;
1553         v4l2_i2c_subdev_init(sd, client, &ov965x_subdev_ops);
1554         strlcpy(sd->name, DRIVER_NAME, sizeof(sd->name));
1555
1556         sd->internal_ops = &ov965x_sd_internal_ops;
1557         sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
1558                      V4L2_SUBDEV_FL_HAS_EVENTS;
1559
1560         ov965x->pad.flags = MEDIA_PAD_FL_SOURCE;
1561         sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
1562         ret = media_entity_pads_init(&sd->entity, 1, &ov965x->pad);
1563         if (ret < 0)
1564                 goto err_mutex;
1565
1566         ret = ov965x_initialize_controls(ov965x);
1567         if (ret < 0)
1568                 goto err_me;
1569
1570         ov965x_get_default_format(&ov965x->format);
1571         ov965x->frame_size = &ov965x_framesizes[0];
1572         ov965x->fiv = &ov965x_intervals[0];
1573
1574         ret = ov965x_detect_sensor(sd);
1575         if (ret < 0)
1576                 goto err_ctrls;
1577
1578         /* Update exposure time min/max to match frame format */
1579         ov965x_update_exposure_ctrl(ov965x);
1580
1581         ret = v4l2_async_register_subdev(sd);
1582         if (ret < 0)
1583                 goto err_ctrls;
1584
1585         return 0;
1586 err_ctrls:
1587         v4l2_ctrl_handler_free(sd->ctrl_handler);
1588 err_me:
1589         media_entity_cleanup(&sd->entity);
1590 err_mutex:
1591         mutex_destroy(&ov965x->lock);
1592         return ret;
1593 }
1594
1595 static int ov965x_remove(struct i2c_client *client)
1596 {
1597         struct v4l2_subdev *sd = i2c_get_clientdata(client);
1598         struct ov965x *ov965x = to_ov965x(sd);
1599
1600         v4l2_async_unregister_subdev(sd);
1601         v4l2_ctrl_handler_free(sd->ctrl_handler);
1602         media_entity_cleanup(&sd->entity);
1603         mutex_destroy(&ov965x->lock);
1604
1605         return 0;
1606 }
1607
1608 static const struct i2c_device_id ov965x_id[] = {
1609         { "OV9650", 0 },
1610         { "OV9652", 0 },
1611         { /* sentinel */ }
1612 };
1613 MODULE_DEVICE_TABLE(i2c, ov965x_id);
1614
1615 #if IS_ENABLED(CONFIG_OF)
1616 static const struct of_device_id ov965x_of_match[] = {
1617         { .compatible = "ovti,ov9650", },
1618         { .compatible = "ovti,ov9652", },
1619         { /* sentinel */ }
1620 };
1621 MODULE_DEVICE_TABLE(of, ov965x_of_match);
1622 #endif
1623
1624 static struct i2c_driver ov965x_i2c_driver = {
1625         .driver = {
1626                 .name   = DRIVER_NAME,
1627                 .of_match_table = of_match_ptr(ov965x_of_match),
1628         },
1629         .probe          = ov965x_probe,
1630         .remove         = ov965x_remove,
1631         .id_table       = ov965x_id,
1632 };
1633
1634 module_i2c_driver(ov965x_i2c_driver);
1635
1636 MODULE_AUTHOR("Sylwester Nawrocki <sylvester.nawrocki@gmail.com>");
1637 MODULE_DESCRIPTION("OV9650/OV9652 CMOS Image Sensor driver");
1638 MODULE_LICENSE("GPL");