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