• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
1 /*
2  * vs6624.c ST VS6624 CMOS image sensor driver
3  *
4  * Copyright (c) 2011 Analog Devices Inc.
5  *
6  * This program is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software
17  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
18  */
19 
20 #include <linux/delay.h>
21 #include <linux/errno.h>
22 #include <linux/gpio.h>
23 #include <linux/i2c.h>
24 #include <linux/init.h>
25 #include <linux/module.h>
26 #include <linux/slab.h>
27 #include <linux/types.h>
28 #include <linux/videodev2.h>
29 
30 #include <media/v4l2-ctrls.h>
31 #include <media/v4l2-device.h>
32 #include <media/v4l2-mediabus.h>
33 #include <media/v4l2-image-sizes.h>
34 
35 #include "vs6624_regs.h"
36 
37 #define MAX_FRAME_RATE  30
38 
39 struct vs6624 {
40 	struct v4l2_subdev sd;
41 	struct v4l2_ctrl_handler hdl;
42 	struct v4l2_fract frame_rate;
43 	struct v4l2_mbus_framefmt fmt;
44 	unsigned ce_pin;
45 };
46 
47 static const struct vs6624_format {
48 	enum v4l2_mbus_pixelcode mbus_code;
49 	enum v4l2_colorspace colorspace;
50 } vs6624_formats[] = {
51 	{
52 		.mbus_code      = V4L2_MBUS_FMT_UYVY8_2X8,
53 		.colorspace     = V4L2_COLORSPACE_JPEG,
54 	},
55 	{
56 		.mbus_code      = V4L2_MBUS_FMT_YUYV8_2X8,
57 		.colorspace     = V4L2_COLORSPACE_JPEG,
58 	},
59 	{
60 		.mbus_code      = V4L2_MBUS_FMT_RGB565_2X8_LE,
61 		.colorspace     = V4L2_COLORSPACE_SRGB,
62 	},
63 };
64 
65 static struct v4l2_mbus_framefmt vs6624_default_fmt = {
66 	.width = VGA_WIDTH,
67 	.height = VGA_HEIGHT,
68 	.code = V4L2_MBUS_FMT_UYVY8_2X8,
69 	.field = V4L2_FIELD_NONE,
70 	.colorspace = V4L2_COLORSPACE_JPEG,
71 };
72 
73 static const u16 vs6624_p1[] = {
74 	0x8104, 0x03,
75 	0x8105, 0x01,
76 	0xc900, 0x03,
77 	0xc904, 0x47,
78 	0xc905, 0x10,
79 	0xc906, 0x80,
80 	0xc907, 0x3a,
81 	0x903a, 0x02,
82 	0x903b, 0x47,
83 	0x903c, 0x15,
84 	0xc908, 0x31,
85 	0xc909, 0xdc,
86 	0xc90a, 0x80,
87 	0xc90b, 0x44,
88 	0x9044, 0x02,
89 	0x9045, 0x31,
90 	0x9046, 0xe2,
91 	0xc90c, 0x07,
92 	0xc90d, 0xe0,
93 	0xc90e, 0x80,
94 	0xc90f, 0x47,
95 	0x9047, 0x90,
96 	0x9048, 0x83,
97 	0x9049, 0x81,
98 	0x904a, 0xe0,
99 	0x904b, 0x60,
100 	0x904c, 0x08,
101 	0x904d, 0x90,
102 	0x904e, 0xc0,
103 	0x904f, 0x43,
104 	0x9050, 0x74,
105 	0x9051, 0x01,
106 	0x9052, 0xf0,
107 	0x9053, 0x80,
108 	0x9054, 0x05,
109 	0x9055, 0xE4,
110 	0x9056, 0x90,
111 	0x9057, 0xc0,
112 	0x9058, 0x43,
113 	0x9059, 0xf0,
114 	0x905a, 0x02,
115 	0x905b, 0x07,
116 	0x905c, 0xec,
117 	0xc910, 0x5d,
118 	0xc911, 0xca,
119 	0xc912, 0x80,
120 	0xc913, 0x5d,
121 	0x905d, 0xa3,
122 	0x905e, 0x04,
123 	0x905f, 0xf0,
124 	0x9060, 0xa3,
125 	0x9061, 0x04,
126 	0x9062, 0xf0,
127 	0x9063, 0x22,
128 	0xc914, 0x72,
129 	0xc915, 0x92,
130 	0xc916, 0x80,
131 	0xc917, 0x64,
132 	0x9064, 0x74,
133 	0x9065, 0x01,
134 	0x9066, 0x02,
135 	0x9067, 0x72,
136 	0x9068, 0x95,
137 	0xc918, 0x47,
138 	0xc919, 0xf2,
139 	0xc91a, 0x81,
140 	0xc91b, 0x69,
141 	0x9169, 0x74,
142 	0x916a, 0x02,
143 	0x916b, 0xf0,
144 	0x916c, 0xec,
145 	0x916d, 0xb4,
146 	0x916e, 0x10,
147 	0x916f, 0x0a,
148 	0x9170, 0x90,
149 	0x9171, 0x80,
150 	0x9172, 0x16,
151 	0x9173, 0xe0,
152 	0x9174, 0x70,
153 	0x9175, 0x04,
154 	0x9176, 0x90,
155 	0x9177, 0xd3,
156 	0x9178, 0xc4,
157 	0x9179, 0xf0,
158 	0x917a, 0x22,
159 	0xc91c, 0x0a,
160 	0xc91d, 0xbe,
161 	0xc91e, 0x80,
162 	0xc91f, 0x73,
163 	0x9073, 0xfc,
164 	0x9074, 0xa3,
165 	0x9075, 0xe0,
166 	0x9076, 0xf5,
167 	0x9077, 0x82,
168 	0x9078, 0x8c,
169 	0x9079, 0x83,
170 	0x907a, 0xa3,
171 	0x907b, 0xa3,
172 	0x907c, 0xe0,
173 	0x907d, 0xfc,
174 	0x907e, 0xa3,
175 	0x907f, 0xe0,
176 	0x9080, 0xc3,
177 	0x9081, 0x9f,
178 	0x9082, 0xff,
179 	0x9083, 0xec,
180 	0x9084, 0x9e,
181 	0x9085, 0xfe,
182 	0x9086, 0x02,
183 	0x9087, 0x0a,
184 	0x9088, 0xea,
185 	0xc920, 0x47,
186 	0xc921, 0x38,
187 	0xc922, 0x80,
188 	0xc923, 0x89,
189 	0x9089, 0xec,
190 	0x908a, 0xd3,
191 	0x908b, 0x94,
192 	0x908c, 0x20,
193 	0x908d, 0x40,
194 	0x908e, 0x01,
195 	0x908f, 0x1c,
196 	0x9090, 0x90,
197 	0x9091, 0xd3,
198 	0x9092, 0xd4,
199 	0x9093, 0xec,
200 	0x9094, 0xf0,
201 	0x9095, 0x02,
202 	0x9096, 0x47,
203 	0x9097, 0x3d,
204 	0xc924, 0x45,
205 	0xc925, 0xca,
206 	0xc926, 0x80,
207 	0xc927, 0x98,
208 	0x9098, 0x12,
209 	0x9099, 0x77,
210 	0x909a, 0xd6,
211 	0x909b, 0x02,
212 	0x909c, 0x45,
213 	0x909d, 0xcd,
214 	0xc928, 0x20,
215 	0xc929, 0xd5,
216 	0xc92a, 0x80,
217 	0xc92b, 0x9e,
218 	0x909e, 0x90,
219 	0x909f, 0x82,
220 	0x90a0, 0x18,
221 	0x90a1, 0xe0,
222 	0x90a2, 0xb4,
223 	0x90a3, 0x03,
224 	0x90a4, 0x0e,
225 	0x90a5, 0x90,
226 	0x90a6, 0x83,
227 	0x90a7, 0xbf,
228 	0x90a8, 0xe0,
229 	0x90a9, 0x60,
230 	0x90aa, 0x08,
231 	0x90ab, 0x90,
232 	0x90ac, 0x81,
233 	0x90ad, 0xfc,
234 	0x90ae, 0xe0,
235 	0x90af, 0xff,
236 	0x90b0, 0xc3,
237 	0x90b1, 0x13,
238 	0x90b2, 0xf0,
239 	0x90b3, 0x90,
240 	0x90b4, 0x81,
241 	0x90b5, 0xfc,
242 	0x90b6, 0xe0,
243 	0x90b7, 0xff,
244 	0x90b8, 0x02,
245 	0x90b9, 0x20,
246 	0x90ba, 0xda,
247 	0xc92c, 0x70,
248 	0xc92d, 0xbc,
249 	0xc92e, 0x80,
250 	0xc92f, 0xbb,
251 	0x90bb, 0x90,
252 	0x90bc, 0x82,
253 	0x90bd, 0x18,
254 	0x90be, 0xe0,
255 	0x90bf, 0xb4,
256 	0x90c0, 0x03,
257 	0x90c1, 0x06,
258 	0x90c2, 0x90,
259 	0x90c3, 0xc1,
260 	0x90c4, 0x06,
261 	0x90c5, 0x74,
262 	0x90c6, 0x05,
263 	0x90c7, 0xf0,
264 	0x90c8, 0x90,
265 	0x90c9, 0xd3,
266 	0x90ca, 0xa0,
267 	0x90cb, 0x02,
268 	0x90cc, 0x70,
269 	0x90cd, 0xbf,
270 	0xc930, 0x72,
271 	0xc931, 0x21,
272 	0xc932, 0x81,
273 	0xc933, 0x3b,
274 	0x913b, 0x7d,
275 	0x913c, 0x02,
276 	0x913d, 0x7f,
277 	0x913e, 0x7b,
278 	0x913f, 0x02,
279 	0x9140, 0x72,
280 	0x9141, 0x25,
281 	0xc934, 0x28,
282 	0xc935, 0xae,
283 	0xc936, 0x80,
284 	0xc937, 0xd2,
285 	0x90d2, 0xf0,
286 	0x90d3, 0x90,
287 	0x90d4, 0xd2,
288 	0x90d5, 0x0a,
289 	0x90d6, 0x02,
290 	0x90d7, 0x28,
291 	0x90d8, 0xb4,
292 	0xc938, 0x28,
293 	0xc939, 0xb1,
294 	0xc93a, 0x80,
295 	0xc93b, 0xd9,
296 	0x90d9, 0x90,
297 	0x90da, 0x83,
298 	0x90db, 0xba,
299 	0x90dc, 0xe0,
300 	0x90dd, 0xff,
301 	0x90de, 0x90,
302 	0x90df, 0xd2,
303 	0x90e0, 0x08,
304 	0x90e1, 0xe0,
305 	0x90e2, 0xe4,
306 	0x90e3, 0xef,
307 	0x90e4, 0xf0,
308 	0x90e5, 0xa3,
309 	0x90e6, 0xe0,
310 	0x90e7, 0x74,
311 	0x90e8, 0xff,
312 	0x90e9, 0xf0,
313 	0x90ea, 0x90,
314 	0x90eb, 0xd2,
315 	0x90ec, 0x0a,
316 	0x90ed, 0x02,
317 	0x90ee, 0x28,
318 	0x90ef, 0xb4,
319 	0xc93c, 0x29,
320 	0xc93d, 0x79,
321 	0xc93e, 0x80,
322 	0xc93f, 0xf0,
323 	0x90f0, 0xf0,
324 	0x90f1, 0x90,
325 	0x90f2, 0xd2,
326 	0x90f3, 0x0e,
327 	0x90f4, 0x02,
328 	0x90f5, 0x29,
329 	0x90f6, 0x7f,
330 	0xc940, 0x29,
331 	0xc941, 0x7c,
332 	0xc942, 0x80,
333 	0xc943, 0xf7,
334 	0x90f7, 0x90,
335 	0x90f8, 0x83,
336 	0x90f9, 0xba,
337 	0x90fa, 0xe0,
338 	0x90fb, 0xff,
339 	0x90fc, 0x90,
340 	0x90fd, 0xd2,
341 	0x90fe, 0x0c,
342 	0x90ff, 0xe0,
343 	0x9100, 0xe4,
344 	0x9101, 0xef,
345 	0x9102, 0xf0,
346 	0x9103, 0xa3,
347 	0x9104, 0xe0,
348 	0x9105, 0x74,
349 	0x9106, 0xff,
350 	0x9107, 0xf0,
351 	0x9108, 0x90,
352 	0x9109, 0xd2,
353 	0x910a, 0x0e,
354 	0x910b, 0x02,
355 	0x910c, 0x29,
356 	0x910d, 0x7f,
357 	0xc944, 0x2a,
358 	0xc945, 0x42,
359 	0xc946, 0x81,
360 	0xc947, 0x0e,
361 	0x910e, 0xf0,
362 	0x910f, 0x90,
363 	0x9110, 0xd2,
364 	0x9111, 0x12,
365 	0x9112, 0x02,
366 	0x9113, 0x2a,
367 	0x9114, 0x48,
368 	0xc948, 0x2a,
369 	0xc949, 0x45,
370 	0xc94a, 0x81,
371 	0xc94b, 0x15,
372 	0x9115, 0x90,
373 	0x9116, 0x83,
374 	0x9117, 0xba,
375 	0x9118, 0xe0,
376 	0x9119, 0xff,
377 	0x911a, 0x90,
378 	0x911b, 0xd2,
379 	0x911c, 0x10,
380 	0x911d, 0xe0,
381 	0x911e, 0xe4,
382 	0x911f, 0xef,
383 	0x9120, 0xf0,
384 	0x9121, 0xa3,
385 	0x9122, 0xe0,
386 	0x9123, 0x74,
387 	0x9124, 0xff,
388 	0x9125, 0xf0,
389 	0x9126, 0x90,
390 	0x9127, 0xd2,
391 	0x9128, 0x12,
392 	0x9129, 0x02,
393 	0x912a, 0x2a,
394 	0x912b, 0x48,
395 	0xc900, 0x01,
396 	0x0000, 0x00,
397 };
398 
399 static const u16 vs6624_p2[] = {
400 	0x806f, 0x01,
401 	0x058c, 0x01,
402 	0x0000, 0x00,
403 };
404 
405 static const u16 vs6624_run_setup[] = {
406 	0x1d18, 0x00,				/* Enableconstrainedwhitebalance */
407 	VS6624_PEAK_MIN_OUT_G_MSB, 0x3c,	/* Damper PeakGain Output MSB */
408 	VS6624_PEAK_MIN_OUT_G_LSB, 0x66,	/* Damper PeakGain Output LSB */
409 	VS6624_CM_LOW_THR_MSB, 0x65,		/* Damper Low MSB */
410 	VS6624_CM_LOW_THR_LSB, 0xd1,		/* Damper Low LSB */
411 	VS6624_CM_HIGH_THR_MSB, 0x66,		/* Damper High MSB */
412 	VS6624_CM_HIGH_THR_LSB, 0x62,		/* Damper High LSB */
413 	VS6624_CM_MIN_OUT_MSB, 0x00,		/* Damper Min output MSB */
414 	VS6624_CM_MIN_OUT_LSB, 0x00,		/* Damper Min output LSB */
415 	VS6624_NORA_DISABLE, 0x00,		/* Nora fDisable */
416 	VS6624_NORA_USAGE, 0x04,		/* Nora usage */
417 	VS6624_NORA_LOW_THR_MSB, 0x63,		/* Damper Low MSB Changed 0x63 to 0x65 */
418 	VS6624_NORA_LOW_THR_LSB, 0xd1,		/* Damper Low LSB */
419 	VS6624_NORA_HIGH_THR_MSB, 0x68,		/* Damper High MSB */
420 	VS6624_NORA_HIGH_THR_LSB, 0xdd,		/* Damper High LSB */
421 	VS6624_NORA_MIN_OUT_MSB, 0x3a,		/* Damper Min output MSB */
422 	VS6624_NORA_MIN_OUT_LSB, 0x00,		/* Damper Min output LSB */
423 	VS6624_F2B_DISABLE, 0x00,		/* Disable */
424 	0x1d8a, 0x30,				/* MAXWeightHigh */
425 	0x1d91, 0x62,				/* fpDamperLowThresholdHigh MSB */
426 	0x1d92, 0x4a,				/* fpDamperLowThresholdHigh LSB */
427 	0x1d95, 0x65,				/* fpDamperHighThresholdHigh MSB */
428 	0x1d96, 0x0e,				/* fpDamperHighThresholdHigh LSB */
429 	0x1da1, 0x3a,				/* fpMinimumDamperOutputLow MSB */
430 	0x1da2, 0xb8,				/* fpMinimumDamperOutputLow LSB */
431 	0x1e08, 0x06,				/* MAXWeightLow */
432 	0x1e0a, 0x0a,				/* MAXWeightHigh */
433 	0x1601, 0x3a,				/* Red A MSB */
434 	0x1602, 0x14,				/* Red A LSB */
435 	0x1605, 0x3b,				/* Blue A MSB */
436 	0x1606, 0x85,				/* BLue A LSB */
437 	0x1609, 0x3b,				/* RED B MSB */
438 	0x160a, 0x85,				/* RED B LSB */
439 	0x160d, 0x3a,				/* Blue B MSB */
440 	0x160e, 0x14,				/* Blue B LSB */
441 	0x1611, 0x30,				/* Max Distance from Locus MSB */
442 	0x1612, 0x8f,				/* Max Distance from Locus MSB */
443 	0x1614, 0x01,				/* Enable constrainer */
444 	0x0000, 0x00,
445 };
446 
447 static const u16 vs6624_default[] = {
448 	VS6624_CONTRAST0, 0x84,
449 	VS6624_SATURATION0, 0x75,
450 	VS6624_GAMMA0, 0x11,
451 	VS6624_CONTRAST1, 0x84,
452 	VS6624_SATURATION1, 0x75,
453 	VS6624_GAMMA1, 0x11,
454 	VS6624_MAN_RG, 0x80,
455 	VS6624_MAN_GG, 0x80,
456 	VS6624_MAN_BG, 0x80,
457 	VS6624_WB_MODE, 0x1,
458 	VS6624_EXPO_COMPENSATION, 0xfe,
459 	VS6624_EXPO_METER, 0x0,
460 	VS6624_LIGHT_FREQ, 0x64,
461 	VS6624_PEAK_GAIN, 0xe,
462 	VS6624_PEAK_LOW_THR, 0x28,
463 	VS6624_HMIRROR0, 0x0,
464 	VS6624_VFLIP0, 0x0,
465 	VS6624_ZOOM_HSTEP0_MSB, 0x0,
466 	VS6624_ZOOM_HSTEP0_LSB, 0x1,
467 	VS6624_ZOOM_VSTEP0_MSB, 0x0,
468 	VS6624_ZOOM_VSTEP0_LSB, 0x1,
469 	VS6624_PAN_HSTEP0_MSB, 0x0,
470 	VS6624_PAN_HSTEP0_LSB, 0xf,
471 	VS6624_PAN_VSTEP0_MSB, 0x0,
472 	VS6624_PAN_VSTEP0_LSB, 0xf,
473 	VS6624_SENSOR_MODE, 0x1,
474 	VS6624_SYNC_CODE_SETUP, 0x21,
475 	VS6624_DISABLE_FR_DAMPER, 0x0,
476 	VS6624_FR_DEN, 0x1,
477 	VS6624_FR_NUM_LSB, 0xf,
478 	VS6624_INIT_PIPE_SETUP, 0x0,
479 	VS6624_IMG_FMT0, 0x0,
480 	VS6624_YUV_SETUP, 0x1,
481 	VS6624_IMAGE_SIZE0, 0x2,
482 	0x0000, 0x00,
483 };
484 
to_vs6624(struct v4l2_subdev * sd)485 static inline struct vs6624 *to_vs6624(struct v4l2_subdev *sd)
486 {
487 	return container_of(sd, struct vs6624, sd);
488 }
to_sd(struct v4l2_ctrl * ctrl)489 static inline struct v4l2_subdev *to_sd(struct v4l2_ctrl *ctrl)
490 {
491 	return &container_of(ctrl->handler, struct vs6624, hdl)->sd;
492 }
493 
494 #ifdef CONFIG_VIDEO_ADV_DEBUG
vs6624_read(struct v4l2_subdev * sd,u16 index)495 static int vs6624_read(struct v4l2_subdev *sd, u16 index)
496 {
497 	struct i2c_client *client = v4l2_get_subdevdata(sd);
498 	u8 buf[2];
499 
500 	buf[0] = index >> 8;
501 	buf[1] = index;
502 	i2c_master_send(client, buf, 2);
503 	i2c_master_recv(client, buf, 1);
504 
505 	return buf[0];
506 }
507 #endif
508 
vs6624_write(struct v4l2_subdev * sd,u16 index,u8 value)509 static int vs6624_write(struct v4l2_subdev *sd, u16 index,
510 				u8 value)
511 {
512 	struct i2c_client *client = v4l2_get_subdevdata(sd);
513 	u8 buf[3];
514 
515 	buf[0] = index >> 8;
516 	buf[1] = index;
517 	buf[2] = value;
518 
519 	return i2c_master_send(client, buf, 3);
520 }
521 
vs6624_writeregs(struct v4l2_subdev * sd,const u16 * regs)522 static int vs6624_writeregs(struct v4l2_subdev *sd, const u16 *regs)
523 {
524 	u16 reg;
525 	u8 data;
526 
527 	while (*regs != 0x00) {
528 		reg = *regs++;
529 		data = *regs++;
530 
531 		vs6624_write(sd, reg, data);
532 	}
533 	return 0;
534 }
535 
vs6624_s_ctrl(struct v4l2_ctrl * ctrl)536 static int vs6624_s_ctrl(struct v4l2_ctrl *ctrl)
537 {
538 	struct v4l2_subdev *sd = to_sd(ctrl);
539 
540 	switch (ctrl->id) {
541 	case V4L2_CID_CONTRAST:
542 		vs6624_write(sd, VS6624_CONTRAST0, ctrl->val);
543 		break;
544 	case V4L2_CID_SATURATION:
545 		vs6624_write(sd, VS6624_SATURATION0, ctrl->val);
546 		break;
547 	case V4L2_CID_HFLIP:
548 		vs6624_write(sd, VS6624_HMIRROR0, ctrl->val);
549 		break;
550 	case V4L2_CID_VFLIP:
551 		vs6624_write(sd, VS6624_VFLIP0, ctrl->val);
552 		break;
553 	default:
554 		return -EINVAL;
555 	}
556 
557 	return 0;
558 }
559 
vs6624_enum_mbus_fmt(struct v4l2_subdev * sd,unsigned index,enum v4l2_mbus_pixelcode * code)560 static int vs6624_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned index,
561 				enum v4l2_mbus_pixelcode *code)
562 {
563 	if (index >= ARRAY_SIZE(vs6624_formats))
564 		return -EINVAL;
565 
566 	*code = vs6624_formats[index].mbus_code;
567 	return 0;
568 }
569 
vs6624_try_mbus_fmt(struct v4l2_subdev * sd,struct v4l2_mbus_framefmt * fmt)570 static int vs6624_try_mbus_fmt(struct v4l2_subdev *sd,
571 				struct v4l2_mbus_framefmt *fmt)
572 {
573 	int index;
574 
575 	for (index = 0; index < ARRAY_SIZE(vs6624_formats); index++)
576 		if (vs6624_formats[index].mbus_code == fmt->code)
577 			break;
578 	if (index >= ARRAY_SIZE(vs6624_formats)) {
579 		/* default to first format */
580 		index = 0;
581 		fmt->code = vs6624_formats[0].mbus_code;
582 	}
583 
584 	/* sensor mode is VGA */
585 	if (fmt->width > VGA_WIDTH)
586 		fmt->width = VGA_WIDTH;
587 	if (fmt->height > VGA_HEIGHT)
588 		fmt->height = VGA_HEIGHT;
589 	fmt->width = fmt->width & (~3);
590 	fmt->height = fmt->height & (~3);
591 	fmt->field = V4L2_FIELD_NONE;
592 	fmt->colorspace = vs6624_formats[index].colorspace;
593 	return 0;
594 }
595 
vs6624_s_mbus_fmt(struct v4l2_subdev * sd,struct v4l2_mbus_framefmt * fmt)596 static int vs6624_s_mbus_fmt(struct v4l2_subdev *sd,
597 				struct v4l2_mbus_framefmt *fmt)
598 {
599 	struct vs6624 *sensor = to_vs6624(sd);
600 	int ret;
601 
602 	ret = vs6624_try_mbus_fmt(sd, fmt);
603 	if (ret)
604 		return ret;
605 
606 	/* set image format */
607 	switch (fmt->code) {
608 	case V4L2_MBUS_FMT_UYVY8_2X8:
609 		vs6624_write(sd, VS6624_IMG_FMT0, 0x0);
610 		vs6624_write(sd, VS6624_YUV_SETUP, 0x1);
611 		break;
612 	case V4L2_MBUS_FMT_YUYV8_2X8:
613 		vs6624_write(sd, VS6624_IMG_FMT0, 0x0);
614 		vs6624_write(sd, VS6624_YUV_SETUP, 0x3);
615 		break;
616 	case V4L2_MBUS_FMT_RGB565_2X8_LE:
617 		vs6624_write(sd, VS6624_IMG_FMT0, 0x4);
618 		vs6624_write(sd, VS6624_RGB_SETUP, 0x0);
619 		break;
620 	default:
621 		return -EINVAL;
622 	}
623 
624 	/* set image size */
625 	if ((fmt->width == VGA_WIDTH) && (fmt->height == VGA_HEIGHT))
626 		vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x2);
627 	else if ((fmt->width == QVGA_WIDTH) && (fmt->height == QVGA_HEIGHT))
628 		vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x4);
629 	else if ((fmt->width == QQVGA_WIDTH) && (fmt->height == QQVGA_HEIGHT))
630 		vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x6);
631 	else if ((fmt->width == CIF_WIDTH) && (fmt->height == CIF_HEIGHT))
632 		vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x3);
633 	else if ((fmt->width == QCIF_WIDTH) && (fmt->height == QCIF_HEIGHT))
634 		vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x5);
635 	else if ((fmt->width == QQCIF_WIDTH) && (fmt->height == QQCIF_HEIGHT))
636 		vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x7);
637 	else {
638 		vs6624_write(sd, VS6624_IMAGE_SIZE0, 0x8);
639 		vs6624_write(sd, VS6624_MAN_HSIZE0_MSB, fmt->width >> 8);
640 		vs6624_write(sd, VS6624_MAN_HSIZE0_LSB, fmt->width & 0xFF);
641 		vs6624_write(sd, VS6624_MAN_VSIZE0_MSB, fmt->height >> 8);
642 		vs6624_write(sd, VS6624_MAN_VSIZE0_LSB, fmt->height & 0xFF);
643 		vs6624_write(sd, VS6624_CROP_CTRL0, 0x1);
644 	}
645 
646 	sensor->fmt = *fmt;
647 
648 	return 0;
649 }
650 
vs6624_g_mbus_fmt(struct v4l2_subdev * sd,struct v4l2_mbus_framefmt * fmt)651 static int vs6624_g_mbus_fmt(struct v4l2_subdev *sd,
652 				struct v4l2_mbus_framefmt *fmt)
653 {
654 	struct vs6624 *sensor = to_vs6624(sd);
655 
656 	*fmt = sensor->fmt;
657 	return 0;
658 }
659 
vs6624_g_parm(struct v4l2_subdev * sd,struct v4l2_streamparm * parms)660 static int vs6624_g_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
661 {
662 	struct vs6624 *sensor = to_vs6624(sd);
663 	struct v4l2_captureparm *cp = &parms->parm.capture;
664 
665 	if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
666 		return -EINVAL;
667 
668 	memset(cp, 0, sizeof(*cp));
669 	cp->capability = V4L2_CAP_TIMEPERFRAME;
670 	cp->timeperframe.numerator = sensor->frame_rate.denominator;
671 	cp->timeperframe.denominator = sensor->frame_rate.numerator;
672 	return 0;
673 }
674 
vs6624_s_parm(struct v4l2_subdev * sd,struct v4l2_streamparm * parms)675 static int vs6624_s_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms)
676 {
677 	struct vs6624 *sensor = to_vs6624(sd);
678 	struct v4l2_captureparm *cp = &parms->parm.capture;
679 	struct v4l2_fract *tpf = &cp->timeperframe;
680 
681 	if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
682 		return -EINVAL;
683 	if (cp->extendedmode != 0)
684 		return -EINVAL;
685 
686 	if (tpf->numerator == 0 || tpf->denominator == 0
687 		|| (tpf->denominator > tpf->numerator * MAX_FRAME_RATE)) {
688 		/* reset to max frame rate */
689 		tpf->numerator = 1;
690 		tpf->denominator = MAX_FRAME_RATE;
691 	}
692 	sensor->frame_rate.numerator = tpf->denominator;
693 	sensor->frame_rate.denominator = tpf->numerator;
694 	vs6624_write(sd, VS6624_DISABLE_FR_DAMPER, 0x0);
695 	vs6624_write(sd, VS6624_FR_NUM_MSB,
696 			sensor->frame_rate.numerator >> 8);
697 	vs6624_write(sd, VS6624_FR_NUM_LSB,
698 			sensor->frame_rate.numerator & 0xFF);
699 	vs6624_write(sd, VS6624_FR_DEN,
700 			sensor->frame_rate.denominator & 0xFF);
701 	return 0;
702 }
703 
vs6624_s_stream(struct v4l2_subdev * sd,int enable)704 static int vs6624_s_stream(struct v4l2_subdev *sd, int enable)
705 {
706 	if (enable)
707 		vs6624_write(sd, VS6624_USER_CMD, 0x2);
708 	else
709 		vs6624_write(sd, VS6624_USER_CMD, 0x4);
710 	udelay(100);
711 	return 0;
712 }
713 
714 #ifdef CONFIG_VIDEO_ADV_DEBUG
vs6624_g_register(struct v4l2_subdev * sd,struct v4l2_dbg_register * reg)715 static int vs6624_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
716 {
717 	reg->val = vs6624_read(sd, reg->reg & 0xffff);
718 	reg->size = 1;
719 	return 0;
720 }
721 
vs6624_s_register(struct v4l2_subdev * sd,const struct v4l2_dbg_register * reg)722 static int vs6624_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
723 {
724 	vs6624_write(sd, reg->reg & 0xffff, reg->val & 0xff);
725 	return 0;
726 }
727 #endif
728 
729 static const struct v4l2_ctrl_ops vs6624_ctrl_ops = {
730 	.s_ctrl = vs6624_s_ctrl,
731 };
732 
733 static const struct v4l2_subdev_core_ops vs6624_core_ops = {
734 #ifdef CONFIG_VIDEO_ADV_DEBUG
735 	.g_register = vs6624_g_register,
736 	.s_register = vs6624_s_register,
737 #endif
738 };
739 
740 static const struct v4l2_subdev_video_ops vs6624_video_ops = {
741 	.enum_mbus_fmt = vs6624_enum_mbus_fmt,
742 	.try_mbus_fmt = vs6624_try_mbus_fmt,
743 	.s_mbus_fmt = vs6624_s_mbus_fmt,
744 	.g_mbus_fmt = vs6624_g_mbus_fmt,
745 	.s_parm = vs6624_s_parm,
746 	.g_parm = vs6624_g_parm,
747 	.s_stream = vs6624_s_stream,
748 };
749 
750 static const struct v4l2_subdev_ops vs6624_ops = {
751 	.core = &vs6624_core_ops,
752 	.video = &vs6624_video_ops,
753 };
754 
vs6624_probe(struct i2c_client * client,const struct i2c_device_id * id)755 static int vs6624_probe(struct i2c_client *client,
756 			const struct i2c_device_id *id)
757 {
758 	struct vs6624 *sensor;
759 	struct v4l2_subdev *sd;
760 	struct v4l2_ctrl_handler *hdl;
761 	const unsigned *ce;
762 	int ret;
763 
764 	/* Check if the adapter supports the needed features */
765 	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C))
766 		return -EIO;
767 
768 	ce = client->dev.platform_data;
769 	if (ce == NULL)
770 		return -EINVAL;
771 
772 	ret = devm_gpio_request_one(&client->dev, *ce, GPIOF_OUT_INIT_HIGH,
773 				    "VS6624 Chip Enable");
774 	if (ret) {
775 		v4l_err(client, "failed to request GPIO %d\n", *ce);
776 		return ret;
777 	}
778 	/* wait 100ms before any further i2c writes are performed */
779 	mdelay(100);
780 
781 	sensor = devm_kzalloc(&client->dev, sizeof(*sensor), GFP_KERNEL);
782 	if (sensor == NULL)
783 		return -ENOMEM;
784 
785 	sd = &sensor->sd;
786 	v4l2_i2c_subdev_init(sd, client, &vs6624_ops);
787 
788 	vs6624_writeregs(sd, vs6624_p1);
789 	vs6624_write(sd, VS6624_MICRO_EN, 0x2);
790 	vs6624_write(sd, VS6624_DIO_EN, 0x1);
791 	mdelay(10);
792 	vs6624_writeregs(sd, vs6624_p2);
793 
794 	vs6624_writeregs(sd, vs6624_default);
795 	vs6624_write(sd, VS6624_HSYNC_SETUP, 0xF);
796 	vs6624_writeregs(sd, vs6624_run_setup);
797 
798 	/* set frame rate */
799 	sensor->frame_rate.numerator = MAX_FRAME_RATE;
800 	sensor->frame_rate.denominator = 1;
801 	vs6624_write(sd, VS6624_DISABLE_FR_DAMPER, 0x0);
802 	vs6624_write(sd, VS6624_FR_NUM_MSB,
803 			sensor->frame_rate.numerator >> 8);
804 	vs6624_write(sd, VS6624_FR_NUM_LSB,
805 			sensor->frame_rate.numerator & 0xFF);
806 	vs6624_write(sd, VS6624_FR_DEN,
807 			sensor->frame_rate.denominator & 0xFF);
808 
809 	sensor->fmt = vs6624_default_fmt;
810 	sensor->ce_pin = *ce;
811 
812 	v4l_info(client, "chip found @ 0x%02x (%s)\n",
813 			client->addr << 1, client->adapter->name);
814 
815 	hdl = &sensor->hdl;
816 	v4l2_ctrl_handler_init(hdl, 4);
817 	v4l2_ctrl_new_std(hdl, &vs6624_ctrl_ops,
818 			V4L2_CID_CONTRAST, 0, 0xFF, 1, 0x87);
819 	v4l2_ctrl_new_std(hdl, &vs6624_ctrl_ops,
820 			V4L2_CID_SATURATION, 0, 0xFF, 1, 0x78);
821 	v4l2_ctrl_new_std(hdl, &vs6624_ctrl_ops,
822 			V4L2_CID_HFLIP, 0, 1, 1, 0);
823 	v4l2_ctrl_new_std(hdl, &vs6624_ctrl_ops,
824 			V4L2_CID_VFLIP, 0, 1, 1, 0);
825 	/* hook the control handler into the driver */
826 	sd->ctrl_handler = hdl;
827 	if (hdl->error) {
828 		int err = hdl->error;
829 
830 		v4l2_ctrl_handler_free(hdl);
831 		return err;
832 	}
833 
834 	/* initialize the hardware to the default control values */
835 	ret = v4l2_ctrl_handler_setup(hdl);
836 	if (ret)
837 		v4l2_ctrl_handler_free(hdl);
838 	return ret;
839 }
840 
vs6624_remove(struct i2c_client * client)841 static int vs6624_remove(struct i2c_client *client)
842 {
843 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
844 
845 	v4l2_device_unregister_subdev(sd);
846 	v4l2_ctrl_handler_free(sd->ctrl_handler);
847 	return 0;
848 }
849 
850 static const struct i2c_device_id vs6624_id[] = {
851 	{"vs6624", 0},
852 	{},
853 };
854 
855 MODULE_DEVICE_TABLE(i2c, vs6624_id);
856 
857 static struct i2c_driver vs6624_driver = {
858 	.driver = {
859 		.owner  = THIS_MODULE,
860 		.name   = "vs6624",
861 	},
862 	.probe          = vs6624_probe,
863 	.remove         = vs6624_remove,
864 	.id_table       = vs6624_id,
865 };
866 
867 module_i2c_driver(vs6624_driver);
868 
869 MODULE_DESCRIPTION("VS6624 sensor driver");
870 MODULE_AUTHOR("Scott Jiang <Scott.Jiang.Linux@gmail.com>");
871 MODULE_LICENSE("GPL v2");
872