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