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