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