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                 rgain = (gain - ((1 << m) * 16)) / (1 << m);
 720                 rgain |= (((1 << m) - 1) << 4);
 721
 722                 ret = ov965x_write(client, REG_GAIN, rgain & 0xff);
 723                 if (ret < 0)
 724                         return ret;
 725                 ret = ov965x_read(client, REG_VREF, &reg);
 726                 if (ret < 0)
 727                         return ret;
 728                 reg &= ~VREF_GAIN_MASK;
 729                 reg |= (((rgain >> 8) & 0x3) << 6);
 730                 ret = ov965x_write(client, REG_VREF, reg);
 731                 if (ret < 0)
 732                         return ret;
 733                 /* Return updated control's value to userspace */
 734                 ctrls->gain->val = (1 << m) * (16 + (rgain & 0xf));
 735         }
 736
 737         return ret;
 738 }
 739
 740 static int ov965x_set_sharpness(struct ov965x *ov965x, unsigned int value)
 741 {
 742         u8 com14, edge;
 743         int ret;
 744
 745         ret = ov965x_read(ov965x->client, REG_COM14, &com14);
 746         if (ret < 0)
 747                 return ret;
 748         ret = ov965x_read(ov965x->client, REG_EDGE, &edge);
 749         if (ret < 0)
 750                 return ret;
 751         com14 = value ? com14 | COM14_EDGE_EN : com14 & ~COM14_EDGE_EN;
 752         value--;
 753         if (value > 0x0f) {
 754                 com14 |= COM14_EEF_X2;
 755                 value >>= 1;
 756         } else {
 757                 com14 &= ~COM14_EEF_X2;
 758         }
 759         ret = ov965x_write(ov965x->client, REG_COM14, com14);
 760         if (ret < 0)
 761                 return ret;
 762
 763         edge &= ~EDGE_FACTOR_MASK;
 764         edge |= ((u8)value & 0x0f);
 765
 766         return ov965x_write(ov965x->client, REG_EDGE, edge);
 767 }
 768
 769 static int ov965x_set_exposure(struct ov965x *ov965x, int exp)
 770 {
 771         struct i2c_client *client = ov965x->client;
 772         struct ov965x_ctrls *ctrls = &ov965x->ctrls;
 773         bool auto_exposure = (exp == V4L2_EXPOSURE_AUTO);
 774         int ret;
 775         u8 reg;
 776
 777         if (ctrls->auto_exp->is_new) {
 778                 ret = ov965x_read(client, REG_COM8, &reg);
 779                 if (ret < 0)
 780                         return ret;
 781                 if (auto_exposure)
 782                         reg |= (COM8_AEC | COM8_AGC);
 783                 else
 784                         reg &= ~(COM8_AEC | COM8_AGC);
 785                 ret = ov965x_write(client, REG_COM8, reg);
 786                 if (ret < 0)
 787                         return ret;
 788         }
 789
 790         if (!auto_exposure && ctrls->exposure->is_new) {
 791                 unsigned int exposure = (ctrls->exposure->val * 100)
 792                                          / ov965x->exp_row_interval;
 793                 /*
 794                  * Manual exposure value
 795                  * [b15:b0] - AECHM (b15:b10), AECH (b9:b2), COM1 (b1:b0)
 796                  */
 797                 ret = ov965x_write(client, REG_COM1, exposure & 0x3);
 798                 if (!ret)
 799                         ret = ov965x_write(client, REG_AECH,
 800                                            (exposure >> 2) & 0xff);
 801                 if (!ret)
 802                         ret = ov965x_write(client, REG_AECHM,
 803                                            (exposure >> 10) & 0x3f);
 804                 /* Update the value to minimize rounding errors */
 805                 ctrls->exposure->val = ((exposure * ov965x->exp_row_interval)
 806                                                         + 50) / 100;
 807                 if (ret < 0)
 808                         return ret;
 809         }
 810
 811         v4l2_ctrl_activate(ov965x->ctrls.brightness, !exp);
 812         return 0;
 813 }
 814
 815 static int ov965x_set_flip(struct ov965x *ov965x)
 816 {
 817         u8 mvfp = 0;
 818
 819         if (ov965x->ctrls.hflip->val)
 820                 mvfp |= MVFP_MIRROR;
 821
 822         if (ov965x->ctrls.vflip->val)
 823                 mvfp |= MVFP_FLIP;
 824
 825         return ov965x_write(ov965x->client, REG_MVFP, mvfp);
 826 }
 827
 828 #define NUM_SAT_LEVELS  5
 829 #define NUM_SAT_REGS    6
 830
 831 static int ov965x_set_saturation(struct ov965x *ov965x, int val)
 832 {
 833         static const u8 regs[NUM_SAT_LEVELS][NUM_SAT_REGS] = {
 834                 /* MTX(1)...MTX(6) */
 835                 { 0x1d, 0x1f, 0x02, 0x09, 0x13, 0x1c }, /* -2 */
 836                 { 0x2e, 0x31, 0x02, 0x0e, 0x1e, 0x2d }, /* -1 */
 837                 { 0x3a, 0x3d, 0x03, 0x12, 0x26, 0x38 }, /*  0 */
 838                 { 0x46, 0x49, 0x04, 0x16, 0x2e, 0x43 }, /* +1 */
 839                 { 0x57, 0x5c, 0x05, 0x1b, 0x39, 0x54 }, /* +2 */
 840         };
 841         u8 addr = REG_MTX(1);
 842         int i, ret = 0;
 843
 844         val += (NUM_SAT_LEVELS / 2);
 845         if (val >= NUM_SAT_LEVELS)
 846                 return -EINVAL;
 847
 848         for (i = 0; i < NUM_SAT_REGS && !ret; i++)
 849                 ret = ov965x_write(ov965x->client, addr + i, regs[val][i]);
 850
 851         return ret;
 852 }
 853
 854 static int ov965x_set_test_pattern(struct ov965x *ov965x, int value)
 855 {
 856         int ret;
 857         u8 reg;
 858
 859         ret = ov965x_read(ov965x->client, REG_COM23, &reg);
 860         if (ret < 0)
 861                 return ret;
 862         reg = value ? reg | COM23_TEST_MODE : reg & ~COM23_TEST_MODE;
 863         return ov965x_write(ov965x->client, REG_COM23, reg);
 864 }
 865
 866 static int __g_volatile_ctrl(struct ov965x *ov965x, struct v4l2_ctrl *ctrl)
 867 {
 868         struct i2c_client *client = ov965x->client;
 869         unsigned int exposure, gain, m;
 870         u8 reg0, reg1, reg2;
 871         int ret;
 872
 873         if (!ov965x->power)
 874                 return 0;
 875
 876         switch (ctrl->id) {
 877         case V4L2_CID_AUTOGAIN:
 878                 if (!ctrl->val)
 879                         return 0;
 880                 ret = ov965x_read(client, REG_GAIN, &reg0);
 881                 if (ret < 0)
 882                         return ret;
 883                 ret = ov965x_read(client, REG_VREF, &reg1);
 884                 if (ret < 0)
 885                         return ret;
 886                 gain = ((reg1 >> 6) << 8) | reg0;
 887                 m = 0x01 << fls(gain >> 4);
 888                 ov965x->ctrls.gain->val = m * (16 + (gain & 0xf));
 889                 break;
 890
 891         case V4L2_CID_EXPOSURE_AUTO:
 892                 if (ctrl->val == V4L2_EXPOSURE_MANUAL)
 893                         return 0;
 894                 ret = ov965x_read(client, REG_COM1, &reg0);
 895                 if (ret < 0)
 896                         return ret;
 897                 ret = ov965x_read(client, REG_AECH, &reg1);
 898                 if (ret < 0)
 899                         return ret;
 900                 ret = ov965x_read(client, REG_AECHM, &reg2);
 901                 if (ret < 0)
 902                         return ret;
 903                 exposure = ((reg2 & 0x3f) << 10) | (reg1 << 2) |
 904                                                 (reg0 & 0x3);
 905                 ov965x->ctrls.exposure->val = ((exposure *
 906                                 ov965x->exp_row_interval) + 50) / 100;
 907                 break;
 908         }
 909
 910         return 0;
 911 }
 912
 913 static int ov965x_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 914 {
 915         struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
 916         struct ov965x *ov965x = to_ov965x(sd);
 917         int ret;
 918
 919         v4l2_dbg(1, debug, sd, "g_ctrl: %s\n", ctrl->name);
 920
 921         mutex_lock(&ov965x->lock);
 922         ret = __g_volatile_ctrl(ov965x, ctrl);
 923         mutex_unlock(&ov965x->lock);
 924         return ret;
 925 }
 926
 927 static int ov965x_s_ctrl(struct v4l2_ctrl *ctrl)
 928 {
 929         struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
 930         struct ov965x *ov965x = to_ov965x(sd);
 931         int ret = -EINVAL;
 932
 933         v4l2_dbg(1, debug, sd, "s_ctrl: %s, value: %d. power: %d\n",
 934                  ctrl->name, ctrl->val, ov965x->power);
 935
 936         mutex_lock(&ov965x->lock);
 937         /*
 938          * If the device is not powered up now postpone applying control's
 939          * value to the hardware, until it is ready to accept commands.
 940          */
 941         if (ov965x->power == 0) {
 942                 mutex_unlock(&ov965x->lock);
 943                 return 0;
 944         }
 945
 946         switch (ctrl->id) {
 947         case V4L2_CID_AUTO_WHITE_BALANCE:
 948                 ret = ov965x_set_white_balance(ov965x, ctrl->val);
 949                 break;
 950
 951         case V4L2_CID_BRIGHTNESS:
 952                 ret = ov965x_set_brightness(ov965x, ctrl->val);
 953                 break;
 954
 955         case V4L2_CID_EXPOSURE_AUTO:
 956                 ret = ov965x_set_exposure(ov965x, ctrl->val);
 957                 break;
 958
 959         case V4L2_CID_AUTOGAIN:
 960                 ret = ov965x_set_gain(ov965x, ctrl->val);
 961                 break;
 962
 963         case V4L2_CID_HFLIP:
 964                 ret = ov965x_set_flip(ov965x);
 965                 break;
 966
 967         case V4L2_CID_POWER_LINE_FREQUENCY:
 968                 ret = ov965x_set_banding_filter(ov965x, ctrl->val);
 969                 break;
 970
 971         case V4L2_CID_SATURATION:
 972                 ret = ov965x_set_saturation(ov965x, ctrl->val);
 973                 break;
 974
 975         case V4L2_CID_SHARPNESS:
 976                 ret = ov965x_set_sharpness(ov965x, ctrl->val);
 977                 break;
 978
 979         case V4L2_CID_TEST_PATTERN:
 980                 ret = ov965x_set_test_pattern(ov965x, ctrl->val);
 981                 break;
 982         }
 983
 984         mutex_unlock(&ov965x->lock);
 985         return ret;
 986 }
 987
 988 static const struct v4l2_ctrl_ops ov965x_ctrl_ops = {
 989         .g_volatile_ctrl = ov965x_g_volatile_ctrl,
 990         .s_ctrl = ov965x_s_ctrl,
 991 };
 992
 993 static const char * const test_pattern_menu[] = {
 994         "Disabled",
 995         "Color bars",
 996 };
 997
 998 static int ov965x_initialize_controls(struct ov965x *ov965x)
 999 {
1000         const struct v4l2_ctrl_ops *ops = &ov965x_ctrl_ops;
1001         struct ov965x_ctrls *ctrls = &ov965x->ctrls;
1002         struct v4l2_ctrl_handler *hdl = &ctrls->handler;
1003         int ret;
1004
1005         ret = v4l2_ctrl_handler_init(hdl, 16);
1006         if (ret < 0)
1007                 return ret;
1008
1009         /* Auto/manual white balance */
1010         ctrls->auto_wb = v4l2_ctrl_new_std(hdl, ops,
1011                                            V4L2_CID_AUTO_WHITE_BALANCE,
1012                                            0, 1, 1, 1);
1013         ctrls->blue_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BLUE_BALANCE,
1014                                                 0, 0xff, 1, 0x80);
1015         ctrls->red_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_RED_BALANCE,
1016                                                0, 0xff, 1, 0x80);
1017         /* Auto/manual exposure */
1018         ctrls->auto_exp =
1019                 v4l2_ctrl_new_std_menu(hdl, ops,
1020                                        V4L2_CID_EXPOSURE_AUTO,
1021                                        V4L2_EXPOSURE_MANUAL, 0,
1022                                        V4L2_EXPOSURE_AUTO);
1023         /* Exposure time, in 100 us units. min/max is updated dynamically. */
1024         ctrls->exposure = v4l2_ctrl_new_std(hdl, ops,
1025                                             V4L2_CID_EXPOSURE_ABSOLUTE,
1026                                             2, 1500, 1, 500);
1027         /* Auto/manual gain */
1028         ctrls->auto_gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTOGAIN,
1029                                              0, 1, 1, 1);
1030         ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN,
1031                                         16, 64 * (16 + 15), 1, 64 * 16);
1032
1033         ctrls->saturation = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SATURATION,
1034                                               -2, 2, 1, 0);
1035         ctrls->brightness = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BRIGHTNESS,
1036                                               -3, 3, 1, 0);
1037         ctrls->sharpness = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SHARPNESS,
1038                                              0, 32, 1, 6);
1039
1040         ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
1041         ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
1042
1043         ctrls->light_freq =
1044                 v4l2_ctrl_new_std_menu(hdl, ops,
1045                                        V4L2_CID_POWER_LINE_FREQUENCY,
1046                                        V4L2_CID_POWER_LINE_FREQUENCY_60HZ, ~0x7,
1047                                        V4L2_CID_POWER_LINE_FREQUENCY_50HZ);
1048
1049         v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
1050                                      ARRAY_SIZE(test_pattern_menu) - 1, 0, 0,
1051                                      test_pattern_menu);
1052         if (hdl->error) {
1053                 ret = hdl->error;
1054                 v4l2_ctrl_handler_free(hdl);
1055                 return ret;
1056         }
1057
1058         ctrls->gain->flags |= V4L2_CTRL_FLAG_VOLATILE;
1059         ctrls->exposure->flags |= V4L2_CTRL_FLAG_VOLATILE;
1060
1061         v4l2_ctrl_auto_cluster(3, &ctrls->auto_wb, 0, false);
1062         v4l2_ctrl_auto_cluster(2, &ctrls->auto_gain, 0, true);
1063         v4l2_ctrl_auto_cluster(2, &ctrls->auto_exp, 1, true);
1064         v4l2_ctrl_cluster(2, &ctrls->hflip);
1065
1066         ov965x->sd.ctrl_handler = hdl;
1067         return 0;
1068 }
1069
1070 /*
1071  * V4L2 subdev video and pad level operations
1072  */
1073 static void ov965x_get_default_format(struct v4l2_mbus_framefmt *mf)
1074 {
1075         mf->width = ov965x_framesizes[0].width;
1076         mf->height = ov965x_framesizes[0].height;
1077         mf->colorspace = ov965x_formats[0].colorspace;
1078         mf->code = ov965x_formats[0].code;
1079         mf->field = V4L2_FIELD_NONE;
1080 }
1081
1082 static int ov965x_enum_mbus_code(struct v4l2_subdev *sd,
1083                                  struct v4l2_subdev_pad_config *cfg,
1084                                  struct v4l2_subdev_mbus_code_enum *code)
1085 {
1086         if (code->index >= ARRAY_SIZE(ov965x_formats))
1087                 return -EINVAL;
1088
1089         code->code = ov965x_formats[code->index].code;
1090         return 0;
1091 }
1092
1093 static int ov965x_enum_frame_sizes(struct v4l2_subdev *sd,
1094                                    struct v4l2_subdev_pad_config *cfg,
1095                                    struct v4l2_subdev_frame_size_enum *fse)
1096 {
1097         int i = ARRAY_SIZE(ov965x_formats);
1098
1099         if (fse->index >= ARRAY_SIZE(ov965x_framesizes))
1100                 return -EINVAL;
1101
1102         while (--i)
1103                 if (fse->code == ov965x_formats[i].code)
1104                         break;
1105
1106         fse->code = ov965x_formats[i].code;
1107
1108         fse->min_width  = ov965x_framesizes[fse->index].width;
1109         fse->max_width  = fse->min_width;
1110         fse->max_height = ov965x_framesizes[fse->index].height;
1111         fse->min_height = fse->max_height;
1112
1113         return 0;
1114 }
1115
1116 static int ov965x_g_frame_interval(struct v4l2_subdev *sd,
1117                                    struct v4l2_subdev_frame_interval *fi)
1118 {
1119         struct ov965x *ov965x = to_ov965x(sd);
1120
1121         mutex_lock(&ov965x->lock);
1122         fi->interval = ov965x->fiv->interval;
1123         mutex_unlock(&ov965x->lock);
1124
1125         return 0;
1126 }
1127
1128 static int __ov965x_set_frame_interval(struct ov965x *ov965x,
1129                                        struct v4l2_subdev_frame_interval *fi)
1130 {
1131         struct v4l2_mbus_framefmt *mbus_fmt = &ov965x->format;
1132         const struct ov965x_interval *fiv = &ov965x_intervals[0];
1133         u64 req_int, err, min_err = ~0ULL;
1134         unsigned int i;
1135
1136         if (fi->interval.denominator == 0)
1137                 return -EINVAL;
1138
1139         req_int = (u64)fi->interval.numerator * 10000;
1140         do_div(req_int, fi->interval.denominator);
1141
1142         for (i = 0; i < ARRAY_SIZE(ov965x_intervals); i++) {
1143                 const struct ov965x_interval *iv = &ov965x_intervals[i];
1144
1145                 if (mbus_fmt->width != iv->size.width ||
1146                     mbus_fmt->height != iv->size.height)
1147                         continue;
1148                 err = abs((u64)(iv->interval.numerator * 10000) /
1149                             iv->interval.denominator - req_int);
1150                 if (err < min_err) {
1151                         fiv = iv;
1152                         min_err = err;
1153                 }
1154         }
1155         ov965x->fiv = fiv;
1156
1157         v4l2_dbg(1, debug, &ov965x->sd, "Changed frame interval to %u us\n",
1158                  fiv->interval.numerator * 1000000 / fiv->interval.denominator);
1159
1160         return 0;
1161 }
1162
1163 static int ov965x_s_frame_interval(struct v4l2_subdev *sd,
1164                                    struct v4l2_subdev_frame_interval *fi)
1165 {
1166         struct ov965x *ov965x = to_ov965x(sd);
1167         int ret;
1168
1169         v4l2_dbg(1, debug, sd, "Setting %d/%d frame interval\n",
1170                  fi->interval.numerator, fi->interval.denominator);
1171
1172         mutex_lock(&ov965x->lock);
1173         ret = __ov965x_set_frame_interval(ov965x, fi);
1174         ov965x->apply_frame_fmt = 1;
1175         mutex_unlock(&ov965x->lock);
1176         return ret;
1177 }
1178
1179 static int ov965x_get_fmt(struct v4l2_subdev *sd,
1180                           struct v4l2_subdev_pad_config *cfg,
1181                           struct v4l2_subdev_format *fmt)
1182 {
1183         struct ov965x *ov965x = to_ov965x(sd);
1184         struct v4l2_mbus_framefmt *mf;
1185
1186         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1187                 mf = v4l2_subdev_get_try_format(sd, cfg, 0);
1188                 fmt->format = *mf;
1189                 return 0;
1190         }
1191
1192         mutex_lock(&ov965x->lock);
1193         fmt->format = ov965x->format;
1194         mutex_unlock(&ov965x->lock);
1195
1196         return 0;
1197 }
1198
1199 static void __ov965x_try_frame_size(struct v4l2_mbus_framefmt *mf,
1200                                     const struct ov965x_framesize **size)
1201 {
1202         const struct ov965x_framesize *fsize = &ov965x_framesizes[0],
1203                 *match = NULL;
1204         int i = ARRAY_SIZE(ov965x_framesizes);
1205         unsigned int min_err = UINT_MAX;
1206
1207         while (i--) {
1208                 int err = abs(fsize->width - mf->width)
1209                                 + abs(fsize->height - mf->height);
1210                 if (err < min_err) {
1211                         min_err = err;
1212                         match = fsize;
1213                 }
1214                 fsize++;
1215         }
1216         if (!match)
1217                 match = &ov965x_framesizes[0];
1218         mf->width  = match->width;
1219         mf->height = match->height;
1220         if (size)
1221                 *size = match;
1222 }
1223
1224 static int ov965x_set_fmt(struct v4l2_subdev *sd,
1225                           struct v4l2_subdev_pad_config *cfg,
1226                           struct v4l2_subdev_format *fmt)
1227 {
1228         unsigned int index = ARRAY_SIZE(ov965x_formats);
1229         struct v4l2_mbus_framefmt *mf = &fmt->format;
1230         struct ov965x *ov965x = to_ov965x(sd);
1231         const struct ov965x_framesize *size = NULL;
1232         int ret = 0;
1233
1234         __ov965x_try_frame_size(mf, &size);
1235
1236         while (--index)
1237                 if (ov965x_formats[index].code == mf->code)
1238                         break;
1239
1240         mf->colorspace  = V4L2_COLORSPACE_JPEG;
1241         mf->code        = ov965x_formats[index].code;
1242         mf->field       = V4L2_FIELD_NONE;
1243
1244         mutex_lock(&ov965x->lock);
1245
1246         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
1247                 if (cfg) {
1248                         mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
1249                         *mf = fmt->format;
1250                 }
1251         } else {
1252                 if (ov965x->streaming) {
1253                         ret = -EBUSY;
1254                 } else {
1255                         ov965x->frame_size = size;
1256                         ov965x->format = fmt->format;
1257                         ov965x->tslb_reg = ov965x_formats[index].tslb_reg;
1258                         ov965x->apply_frame_fmt = 1;
1259                 }
1260         }
1261
1262         if (!ret && fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
1263                 struct v4l2_subdev_frame_interval fiv = {
1264                         .interval = { 0, 1 }
1265                 };
1266                 /* Reset to minimum possible frame interval */
1267                 __ov965x_set_frame_interval(ov965x, &fiv);
1268         }
1269         mutex_unlock(&ov965x->lock);
1270
1271         if (!ret)
1272                 ov965x_update_exposure_ctrl(ov965x);
1273
1274         return ret;
1275 }
1276
1277 static int ov965x_set_frame_size(struct ov965x *ov965x)
1278 {
1279         int i, ret = 0;
1280
1281         for (i = 0; ret == 0 && i < NUM_FMT_REGS; i++)
1282                 ret = ov965x_write(ov965x->client, frame_size_reg_addr[i],
1283                                    ov965x->frame_size->regs[i]);
1284         return ret;
1285 }
1286
1287 static int __ov965x_set_params(struct ov965x *ov965x)
1288 {
1289         struct i2c_client *client = ov965x->client;
1290         struct ov965x_ctrls *ctrls = &ov965x->ctrls;
1291         int ret = 0;
1292         u8 reg;
1293
1294         if (ov965x->apply_frame_fmt) {
1295                 reg = DEF_CLKRC + ov965x->fiv->clkrc_div;
1296                 ret = ov965x_write(client, REG_CLKRC, reg);
1297                 if (ret < 0)
1298                         return ret;
1299                 ret = ov965x_set_frame_size(ov965x);
1300                 if (ret < 0)
1301                         return ret;
1302                 ret = ov965x_read(client, REG_TSLB, &reg);
1303                 if (ret < 0)
1304                         return ret;
1305                 reg &= ~TSLB_YUYV_MASK;
1306                 reg |= ov965x->tslb_reg;
1307                 ret = ov965x_write(client, REG_TSLB, reg);
1308                 if (ret < 0)
1309                         return ret;
1310         }
1311         ret = ov965x_set_default_gamma_curve(ov965x);
1312         if (ret < 0)
1313                 return ret;
1314         ret = ov965x_set_color_matrix(ov965x);
1315         if (ret < 0)
1316                 return ret;
1317         /*
1318          * Select manual banding filter, the filter will
1319          * be enabled further if required.
1320          */
1321         ret = ov965x_read(client, REG_COM11, &reg);
1322         if (!ret)
1323                 reg |= COM11_BANDING;
1324         ret = ov965x_write(client, REG_COM11, reg);
1325         if (ret < 0)
1326                 return ret;
1327         /*
1328          * Banding filter (REG_MBD value) needs to match selected
1329          * resolution and frame rate, so it's always updated here.
1330          */
1331         return ov965x_set_banding_filter(ov965x, ctrls->light_freq->val);
1332 }
1333
1334 static int ov965x_s_stream(struct v4l2_subdev *sd, int on)
1335 {
1336         struct i2c_client *client = v4l2_get_subdevdata(sd);
1337         struct ov965x *ov965x = to_ov965x(sd);
1338         struct ov965x_ctrls *ctrls = &ov965x->ctrls;
1339         int ret = 0;
1340
1341         v4l2_dbg(1, debug, client, "%s: on: %d\n", __func__, on);
1342
1343         mutex_lock(&ov965x->lock);
1344         if (ov965x->streaming == !on) {
1345                 if (on)
1346                         ret = __ov965x_set_params(ov965x);
1347
1348                 if (!ret && ctrls->update) {
1349                         /*
1350                          * ov965x_s_ctrl callback takes the mutex
1351                          * so it needs to be released here.
1352                          */
1353                         mutex_unlock(&ov965x->lock);
1354                         ret = v4l2_ctrl_handler_setup(&ctrls->handler);
1355
1356                         mutex_lock(&ov965x->lock);
1357                         if (!ret)
1358                                 ctrls->update = 0;
1359                 }
1360                 if (!ret)
1361                         ret = ov965x_write(client, REG_COM2,
1362                                            on ? 0x01 : 0x11);
1363         }
1364         if (!ret)
1365                 ov965x->streaming += on ? 1 : -1;
1366
1367         WARN_ON(ov965x->streaming < 0);
1368         mutex_unlock(&ov965x->lock);
1369
1370         return ret;
1371 }
1372
1373 /*
1374  * V4L2 subdev internal operations
1375  */
1376 static int ov965x_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1377 {
1378         struct v4l2_mbus_framefmt *mf =
1379                 v4l2_subdev_get_try_format(sd, fh->pad, 0);
1380
1381         ov965x_get_default_format(mf);
1382         return 0;
1383 }
1384
1385 static const struct v4l2_subdev_pad_ops ov965x_pad_ops = {
1386         .enum_mbus_code = ov965x_enum_mbus_code,
1387         .enum_frame_size = ov965x_enum_frame_sizes,
1388         .get_fmt = ov965x_get_fmt,
1389         .set_fmt = ov965x_set_fmt,
1390 };
1391
1392 static const struct v4l2_subdev_video_ops ov965x_video_ops = {
1393         .s_stream = ov965x_s_stream,
1394         .g_frame_interval = ov965x_g_frame_interval,
1395         .s_frame_interval = ov965x_s_frame_interval,
1396
1397 };
1398
1399 static const struct v4l2_subdev_internal_ops ov965x_sd_internal_ops = {
1400         .open = ov965x_open,
1401 };
1402
1403 static const struct v4l2_subdev_core_ops ov965x_core_ops = {
1404         .s_power = ov965x_s_power,
1405         .log_status = v4l2_ctrl_subdev_log_status,
1406         .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
1407         .unsubscribe_event = v4l2_event_subdev_unsubscribe,
1408 };
1409
1410 static const struct v4l2_subdev_ops ov965x_subdev_ops = {
1411         .core = &ov965x_core_ops,
1412         .pad = &ov965x_pad_ops,
1413         .video = &ov965x_video_ops,
1414 };
1415
1416 /*
1417  * Reset and power down GPIOs configuration
1418  */
1419 static int ov965x_configure_gpios_pdata(struct ov965x *ov965x,
1420                                 const struct ov9650_platform_data *pdata)
1421 {
1422         int ret, i;
1423         int gpios[NUM_GPIOS];
1424
1425         gpios[GPIO_PWDN] = pdata->gpio_pwdn;
1426         gpios[GPIO_RST]  = pdata->gpio_reset;
1427
1428         for (i = 0; i < ARRAY_SIZE(ov965x->gpios); i++) {
1429                 int gpio = gpios[i];
1430
1431                 if (!gpio_is_valid(gpio))
1432                         continue;
1433                 ret = devm_gpio_request_one(&ov965x->client->dev, gpio,
1434                                             GPIOF_OUT_INIT_HIGH, "OV965X");
1435                 if (ret < 0)
1436                         return ret;
1437                 v4l2_dbg(1, debug, &ov965x->sd, "set gpio %d to 1\n", gpio);
1438
1439                 gpio_set_value_cansleep(gpio, 1);
1440                 gpio_export(gpio, 0);
1441                 ov965x->gpios[i] = gpio_to_desc(gpio);
1442         }
1443
1444         return 0;
1445 }
1446
1447 static int ov965x_configure_gpios(struct ov965x *ov965x)
1448 {
1449         struct device *dev = &ov965x->client->dev;
1450
1451         ov965x->gpios[GPIO_PWDN] = devm_gpiod_get_optional(dev, "powerdown",
1452                                                         GPIOD_OUT_HIGH);
1453         if (IS_ERR(ov965x->gpios[GPIO_PWDN])) {
1454                 dev_info(dev, "can't get %s GPIO\n", "powerdown");
1455                 return PTR_ERR(ov965x->gpios[GPIO_PWDN]);
1456         }
1457
1458         ov965x->gpios[GPIO_RST] = devm_gpiod_get_optional(dev, "reset",
1459                                                         GPIOD_OUT_HIGH);
1460         if (IS_ERR(ov965x->gpios[GPIO_RST])) {
1461                 dev_info(dev, "can't get %s GPIO\n", "reset");
1462                 return PTR_ERR(ov965x->gpios[GPIO_RST]);
1463         }
1464
1465         return 0;
1466 }
1467
1468 static int ov965x_detect_sensor(struct v4l2_subdev *sd)
1469 {
1470         struct i2c_client *client = v4l2_get_subdevdata(sd);
1471         struct ov965x *ov965x = to_ov965x(sd);
1472         u8 pid, ver;
1473         int ret;
1474
1475         mutex_lock(&ov965x->lock);
1476         ret = __ov965x_set_power(ov965x, 1);
1477         if (ret)
1478                 goto out;
1479
1480         msleep(25);
1481
1482         /* Check sensor revision */
1483         ret = ov965x_read(client, REG_PID, &pid);
1484         if (!ret)
1485                 ret = ov965x_read(client, REG_VER, &ver);
1486
1487         __ov965x_set_power(ov965x, 0);
1488
1489         if (!ret) {
1490                 ov965x->id = OV965X_ID(pid, ver);
1491                 if (ov965x->id == OV9650_ID || ov965x->id == OV9652_ID) {
1492                         v4l2_info(sd, "Found OV%04X sensor\n", ov965x->id);
1493                 } else {
1494                         v4l2_err(sd, "Sensor detection failed (%04X, %d)\n",
1495                                  ov965x->id, ret);
1496                         ret = -ENODEV;
1497                 }
1498         }
1499 out:
1500         mutex_unlock(&ov965x->lock);
1501
1502         return ret;
1503 }
1504
1505 static int ov965x_probe(struct i2c_client *client,
1506                         const struct i2c_device_id *id)
1507 {
1508         const struct ov9650_platform_data *pdata = client->dev.platform_data;
1509         struct v4l2_subdev *sd;
1510         struct ov965x *ov965x;
1511         int ret;
1512
1513         ov965x = devm_kzalloc(&client->dev, sizeof(*ov965x), GFP_KERNEL);
1514         if (!ov965x)
1515                 return -ENOMEM;
1516
1517         ov965x->client = client;
1518
1519         if (pdata) {
1520                 if (pdata->mclk_frequency == 0) {
1521                         dev_err(&client->dev, "MCLK frequency not specified\n");
1522                         return -EINVAL;
1523                 }
1524                 ov965x->mclk_frequency = pdata->mclk_frequency;
1525
1526                 ret = ov965x_configure_gpios_pdata(ov965x, pdata);
1527                 if (ret < 0)
1528                         return ret;
1529         } else if (dev_fwnode(&client->dev)) {
1530                 ov965x->clk = devm_clk_get(&ov965x->client->dev, NULL);
1531                 if (IS_ERR(ov965x->clk))
1532                         return PTR_ERR(ov965x->clk);
1533                 ov965x->mclk_frequency = clk_get_rate(ov965x->clk);
1534
1535                 ret = ov965x_configure_gpios(ov965x);
1536                 if (ret < 0)
1537                         return ret;
1538         } else {
1539                 dev_err(&client->dev,
1540                         "Neither platform data nor device property specified\n");
1541
1542                 return -EINVAL;
1543         }
1544
1545         mutex_init(&ov965x->lock);
1546
1547         sd = &ov965x->sd;
1548         v4l2_i2c_subdev_init(sd, client, &ov965x_subdev_ops);
1549         strlcpy(sd->name, DRIVER_NAME, sizeof(sd->name));
1550
1551         sd->internal_ops = &ov965x_sd_internal_ops;
1552         sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
1553                      V4L2_SUBDEV_FL_HAS_EVENTS;
1554
1555         ov965x->pad.flags = MEDIA_PAD_FL_SOURCE;
1556         sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
1557         ret = media_entity_pads_init(&sd->entity, 1, &ov965x->pad);
1558         if (ret < 0)
1559                 goto err_mutex;
1560
1561         ret = ov965x_initialize_controls(ov965x);
1562         if (ret < 0)
1563                 goto err_me;
1564
1565         ov965x_get_default_format(&ov965x->format);
1566         ov965x->frame_size = &ov965x_framesizes[0];
1567         ov965x->fiv = &ov965x_intervals[0];
1568
1569         ret = ov965x_detect_sensor(sd);
1570         if (ret < 0)
1571                 goto err_ctrls;
1572
1573         /* Update exposure time min/max to match frame format */
1574         ov965x_update_exposure_ctrl(ov965x);
1575
1576         ret = v4l2_async_register_subdev(sd);
1577         if (ret < 0)
1578                 goto err_ctrls;
1579
1580         return 0;
1581 err_ctrls:
1582         v4l2_ctrl_handler_free(sd->ctrl_handler);
1583 err_me:
1584         media_entity_cleanup(&sd->entity);
1585 err_mutex:
1586         mutex_destroy(&ov965x->lock);
1587         return ret;
1588 }
1589
1590 static int ov965x_remove(struct i2c_client *client)
1591 {
1592         struct v4l2_subdev *sd = i2c_get_clientdata(client);
1593         struct ov965x *ov965x = to_ov965x(sd);
1594
1595         v4l2_async_unregister_subdev(sd);
1596         v4l2_ctrl_handler_free(sd->ctrl_handler);
1597         media_entity_cleanup(&sd->entity);
1598         mutex_destroy(&ov965x->lock);
1599
1600         return 0;
1601 }
1602
1603 static const struct i2c_device_id ov965x_id[] = {
1604         { "OV9650", 0 },
1605         { "OV9652", 0 },
1606         { /* sentinel */ }
1607 };
1608 MODULE_DEVICE_TABLE(i2c, ov965x_id);
1609
1610 #if IS_ENABLED(CONFIG_OF)
1611 static const struct of_device_id ov965x_of_match[] = {
1612         { .compatible = "ovti,ov9650", },
1613         { .compatible = "ovti,ov9652", },
1614         { /* sentinel */ }
1615 };
1616 MODULE_DEVICE_TABLE(of, ov965x_of_match);
1617 #endif
1618
1619 static struct i2c_driver ov965x_i2c_driver = {
1620         .driver = {
1621                 .name   = DRIVER_NAME,
1622                 .of_match_table = of_match_ptr(ov965x_of_match),
1623         },
1624         .probe          = ov965x_probe,
1625         .remove         = ov965x_remove,
1626         .id_table       = ov965x_id,
1627 };
1628
1629 module_i2c_driver(ov965x_i2c_driver);
1630
1631 MODULE_AUTHOR("Sylwester Nawrocki <sylvester.nawrocki@gmail.com>");
1632 MODULE_DESCRIPTION("OV9650/OV9652 CMOS Image Sensor driver");
1633 MODULE_LICENSE("GPL");