drivers/media/i2c/ov9650.c

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