media: tegra: Tegra V4L2 camera
[linux-2.6.git] / drivers / media / video / ov9740.c
1 /*
2  * OmniVision OV9740 Camera Driver
3  *
4  * Copyright (C) 2011 NVIDIA Corporation
5  *
6  * Based on ov9640 camera driver.
7  *
8  * This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License version 2 as
10  * published by the Free Software Foundation.
11  */
12
13 #include <linux/init.h>
14 #include <linux/module.h>
15 #include <linux/i2c.h>
16 #include <linux/slab.h>
17 #include <media/v4l2-chip-ident.h>
18 #include <media/soc_camera.h>
19
20 #define to_ov9740(sd)           container_of(sd, struct ov9740_priv, subdev)
21
22 /* General Status Registers */
23 #define OV9740_MODEL_ID_HI              0x0000
24 #define OV9740_MODEL_ID_LO              0x0001
25 #define OV9740_REVISION_NUMBER          0x0002
26 #define OV9740_MANUFACTURER_ID          0x0003
27 #define OV9740_SMIA_VERSION             0x0004
28
29 /* General Setup Registers */
30 #define OV9740_MODE_SELECT              0x0100
31 #define OV9740_IMAGE_ORT                0x0101
32 #define OV9740_SOFTWARE_RESET           0x0103
33 #define OV9740_GRP_PARAM_HOLD           0x0104
34 #define OV9740_MSK_CORRUP_FM            0x0105
35
36 /* Timing Setting */
37 #define OV9740_FRM_LENGTH_LN_HI         0x0340 /* VTS */
38 #define OV9740_FRM_LENGTH_LN_LO         0x0341 /* VTS */
39 #define OV9740_LN_LENGTH_PCK_HI         0x0342 /* HTS */
40 #define OV9740_LN_LENGTH_PCK_LO         0x0343 /* HTS */
41 #define OV9740_X_ADDR_START_HI          0x0344
42 #define OV9740_X_ADDR_START_LO          0x0345
43 #define OV9740_Y_ADDR_START_HI          0x0346
44 #define OV9740_Y_ADDR_START_LO          0x0347
45 #define OV9740_X_ADDR_END_HI            0x0348
46 #define OV9740_X_ADDR_END_LO            0x0349
47 #define OV9740_Y_ADDR_END_HI            0x034a
48 #define OV9740_Y_ADDR_END_LO            0x034b
49 #define OV9740_X_OUTPUT_SIZE_HI         0x034c
50 #define OV9740_X_OUTPUT_SIZE_LO         0x034d
51 #define OV9740_Y_OUTPUT_SIZE_HI         0x034e
52 #define OV9740_Y_OUTPUT_SIZE_LO         0x034f
53
54 /* IO Control Registers */
55 #define OV9740_IO_CREL00                0x3002
56 #define OV9740_IO_CREL01                0x3004
57 #define OV9740_IO_CREL02                0x3005
58 #define OV9740_IO_OUTPUT_SEL01          0x3026
59 #define OV9740_IO_OUTPUT_SEL02          0x3027
60
61 /* AWB Registers */
62 #define OV9740_AWB_MANUAL_CTRL          0x3406
63
64 /* Analog Control Registers */
65 #define OV9740_ANALOG_CTRL01            0x3601
66 #define OV9740_ANALOG_CTRL02            0x3602
67 #define OV9740_ANALOG_CTRL03            0x3603
68 #define OV9740_ANALOG_CTRL04            0x3604
69 #define OV9740_ANALOG_CTRL10            0x3610
70 #define OV9740_ANALOG_CTRL12            0x3612
71 #define OV9740_ANALOG_CTRL15            0x3615
72 #define OV9740_ANALOG_CTRL20            0x3620
73 #define OV9740_ANALOG_CTRL21            0x3621
74 #define OV9740_ANALOG_CTRL22            0x3622
75 #define OV9740_ANALOG_CTRL30            0x3630
76 #define OV9740_ANALOG_CTRL31            0x3631
77 #define OV9740_ANALOG_CTRL32            0x3632
78 #define OV9740_ANALOG_CTRL33            0x3633
79
80 /* Sensor Control */
81 #define OV9740_SENSOR_CTRL03            0x3703
82 #define OV9740_SENSOR_CTRL04            0x3704
83 #define OV9740_SENSOR_CTRL05            0x3705
84 #define OV9740_SENSOR_CTRL07            0x3707
85
86 /* Timing Control */
87 #define OV9740_TIMING_CTRL17            0x3817
88 #define OV9740_TIMING_CTRL19            0x3819
89 #define OV9740_TIMING_CTRL33            0x3833
90 #define OV9740_TIMING_CTRL35            0x3835
91
92 /* Banding Filter */
93 #define OV9740_AEC_MAXEXPO_60_H         0x3a02
94 #define OV9740_AEC_MAXEXPO_60_L         0x3a03
95 #define OV9740_AEC_B50_STEP_HI          0x3a08
96 #define OV9740_AEC_B50_STEP_LO          0x3a09
97 #define OV9740_AEC_B60_STEP_HI          0x3a0a
98 #define OV9740_AEC_B60_STEP_LO          0x3a0b
99 #define OV9740_AEC_CTRL0D               0x3a0d
100 #define OV9740_AEC_CTRL0E               0x3a0e
101 #define OV9740_AEC_MAXEXPO_50_H         0x3a14
102 #define OV9740_AEC_MAXEXPO_50_L         0x3a15
103
104 /* AEC/AGC Control */
105 #define OV9740_AEC_ENABLE               0x3503
106 #define OV9740_GAIN_CEILING_01          0x3a18
107 #define OV9740_GAIN_CEILING_02          0x3a19
108 #define OV9740_AEC_HI_THRESHOLD         0x3a11
109 #define OV9740_AEC_3A1A                 0x3a1a
110 #define OV9740_AEC_CTRL1B_WPT2          0x3a1b
111 #define OV9740_AEC_CTRL0F_WPT           0x3a0f
112 #define OV9740_AEC_CTRL10_BPT           0x3a10
113 #define OV9740_AEC_CTRL1E_BPT2          0x3a1e
114 #define OV9740_AEC_LO_THRESHOLD         0x3a1f
115
116 /* BLC Control */
117 #define OV9740_BLC_AUTO_ENABLE          0x4002
118 #define OV9740_BLC_MODE                 0x4005
119
120 /* VFIFO */
121 #define OV9740_VFIFO_READ_START_HI      0x4608
122 #define OV9740_VFIFO_READ_START_LO      0x4609
123
124 /* DVP Control */
125 #define OV9740_DVP_VSYNC_CTRL02         0x4702
126 #define OV9740_DVP_VSYNC_MODE           0x4704
127 #define OV9740_DVP_VSYNC_CTRL06         0x4706
128
129 /* PLL Setting */
130 #define OV9740_PLL_MODE_CTRL01          0x3104
131 #define OV9740_PRE_PLL_CLK_DIV          0x0305
132 #define OV9740_PLL_MULTIPLIER           0x0307
133 #define OV9740_VT_SYS_CLK_DIV           0x0303
134 #define OV9740_VT_PIX_CLK_DIV           0x0301
135 #define OV9740_PLL_CTRL3010             0x3010
136 #define OV9740_VFIFO_CTRL00             0x460e
137
138 /* ISP Control */
139 #define OV9740_ISP_CTRL00               0x5000
140 #define OV9740_ISP_CTRL01               0x5001
141 #define OV9740_ISP_CTRL03               0x5003
142 #define OV9740_ISP_CTRL05               0x5005
143 #define OV9740_ISP_CTRL12               0x5012
144 #define OV9740_ISP_CTRL19               0x5019
145 #define OV9740_ISP_CTRL1A               0x501a
146 #define OV9740_ISP_CTRL1E               0x501e
147 #define OV9740_ISP_CTRL1F               0x501f
148 #define OV9740_ISP_CTRL20               0x5020
149 #define OV9740_ISP_CTRL21               0x5021
150
151 /* AWB */
152 #define OV9740_AWB_CTRL00               0x5180
153 #define OV9740_AWB_CTRL01               0x5181
154 #define OV9740_AWB_CTRL02               0x5182
155 #define OV9740_AWB_CTRL03               0x5183
156 #define OV9740_AWB_ADV_CTRL01           0x5184
157 #define OV9740_AWB_ADV_CTRL02           0x5185
158 #define OV9740_AWB_ADV_CTRL03           0x5186
159 #define OV9740_AWB_ADV_CTRL04           0x5187
160 #define OV9740_AWB_ADV_CTRL05           0x5188
161 #define OV9740_AWB_ADV_CTRL06           0x5189
162 #define OV9740_AWB_ADV_CTRL07           0x518a
163 #define OV9740_AWB_ADV_CTRL08           0x518b
164 #define OV9740_AWB_ADV_CTRL09           0x518c
165 #define OV9740_AWB_ADV_CTRL10           0x518d
166 #define OV9740_AWB_ADV_CTRL11           0x518e
167 #define OV9740_AWB_CTRL0F               0x518f
168 #define OV9740_AWB_CTRL10               0x5190
169 #define OV9740_AWB_CTRL11               0x5191
170 #define OV9740_AWB_CTRL12               0x5192
171 #define OV9740_AWB_CTRL13               0x5193
172 #define OV9740_AWB_CTRL14               0x5194
173
174 /* MIPI Control */
175 #define OV9740_MIPI_CTRL00              0x4800
176 #define OV9740_MIPI_3837                0x3837
177 #define OV9740_MIPI_CTRL01              0x4801
178 #define OV9740_MIPI_CTRL03              0x4803
179 #define OV9740_MIPI_CTRL05              0x4805
180 #define OV9740_VFIFO_RD_CTRL            0x4601
181 #define OV9740_MIPI_CTRL_3012           0x3012
182 #define OV9740_SC_CMMM_MIPI_CTR         0x3014
183
184 #define OV9740_MAX_WIDTH                1280
185 #define OV9740_MAX_HEIGHT               720
186
187 /* Misc. structures */
188 struct ov9740_reg {
189         u16                             reg;
190         u8                              val;
191 };
192
193 struct ov9740_priv {
194         struct v4l2_subdev              subdev;
195
196         int                             ident;
197         u16                             model;
198         u8                              revision;
199         u8                              manid;
200         u8                              smiaver;
201
202         bool                            flag_vflip;
203         bool                            flag_hflip;
204
205         /* For suspend/resume. */
206         struct v4l2_mbus_framefmt       current_mf;
207         bool                            current_enable;
208 };
209
210 static const struct ov9740_reg ov9740_defaults[] = {
211         /* Software Reset */
212         { OV9740_SOFTWARE_RESET,        0x01 },
213
214         /* Banding Filter */
215         { OV9740_AEC_B50_STEP_HI,       0x00 },
216         { OV9740_AEC_B50_STEP_LO,       0xe8 },
217         { OV9740_AEC_CTRL0E,            0x03 },
218         { OV9740_AEC_MAXEXPO_50_H,      0x15 },
219         { OV9740_AEC_MAXEXPO_50_L,      0xc6 },
220         { OV9740_AEC_B60_STEP_HI,       0x00 },
221         { OV9740_AEC_B60_STEP_LO,       0xc0 },
222         { OV9740_AEC_CTRL0D,            0x04 },
223         { OV9740_AEC_MAXEXPO_60_H,      0x18 },
224         { OV9740_AEC_MAXEXPO_60_L,      0x20 },
225
226         /* LC */
227         { 0x5842, 0x02 }, { 0x5843, 0x5e }, { 0x5844, 0x04 }, { 0x5845, 0x32 },
228         { 0x5846, 0x03 }, { 0x5847, 0x29 }, { 0x5848, 0x02 }, { 0x5849, 0xcc },
229
230         /* Un-documented OV9740 registers */
231         { 0x5800, 0x29 }, { 0x5801, 0x25 }, { 0x5802, 0x20 }, { 0x5803, 0x21 },
232         { 0x5804, 0x26 }, { 0x5805, 0x2e }, { 0x5806, 0x11 }, { 0x5807, 0x0c },
233         { 0x5808, 0x09 }, { 0x5809, 0x0a }, { 0x580a, 0x0e }, { 0x580b, 0x16 },
234         { 0x580c, 0x06 }, { 0x580d, 0x02 }, { 0x580e, 0x00 }, { 0x580f, 0x00 },
235         { 0x5810, 0x04 }, { 0x5811, 0x0a }, { 0x5812, 0x05 }, { 0x5813, 0x02 },
236         { 0x5814, 0x00 }, { 0x5815, 0x00 }, { 0x5816, 0x03 }, { 0x5817, 0x09 },
237         { 0x5818, 0x0f }, { 0x5819, 0x0a }, { 0x581a, 0x07 }, { 0x581b, 0x08 },
238         { 0x581c, 0x0b }, { 0x581d, 0x14 }, { 0x581e, 0x28 }, { 0x581f, 0x23 },
239         { 0x5820, 0x1d }, { 0x5821, 0x1e }, { 0x5822, 0x24 }, { 0x5823, 0x2a },
240         { 0x5824, 0x4f }, { 0x5825, 0x6f }, { 0x5826, 0x5f }, { 0x5827, 0x7f },
241         { 0x5828, 0x9f }, { 0x5829, 0x5f }, { 0x582a, 0x8f }, { 0x582b, 0x9e },
242         { 0x582c, 0x8f }, { 0x582d, 0x9f }, { 0x582e, 0x4f }, { 0x582f, 0x87 },
243         { 0x5830, 0x86 }, { 0x5831, 0x97 }, { 0x5832, 0xae }, { 0x5833, 0x3f },
244         { 0x5834, 0x8e }, { 0x5835, 0x7c }, { 0x5836, 0x7e }, { 0x5837, 0xaf },
245         { 0x5838, 0x8f }, { 0x5839, 0x8f }, { 0x583a, 0x9f }, { 0x583b, 0x7f },
246         { 0x583c, 0x5f },
247
248         /* Y Gamma */
249         { 0x5480, 0x07 }, { 0x5481, 0x18 }, { 0x5482, 0x2c }, { 0x5483, 0x4e },
250         { 0x5484, 0x5e }, { 0x5485, 0x6b }, { 0x5486, 0x77 }, { 0x5487, 0x82 },
251         { 0x5488, 0x8c }, { 0x5489, 0x95 }, { 0x548a, 0xa4 }, { 0x548b, 0xb1 },
252         { 0x548c, 0xc6 }, { 0x548d, 0xd8 }, { 0x548e, 0xe9 },
253
254         /* UV Gamma */
255         { 0x5490, 0x0f }, { 0x5491, 0xff }, { 0x5492, 0x0d }, { 0x5493, 0x05 },
256         { 0x5494, 0x07 }, { 0x5495, 0x1a }, { 0x5496, 0x04 }, { 0x5497, 0x01 },
257         { 0x5498, 0x03 }, { 0x5499, 0x53 }, { 0x549a, 0x02 }, { 0x549b, 0xeb },
258         { 0x549c, 0x02 }, { 0x549d, 0xa0 }, { 0x549e, 0x02 }, { 0x549f, 0x67 },
259         { 0x54a0, 0x02 }, { 0x54a1, 0x3b }, { 0x54a2, 0x02 }, { 0x54a3, 0x18 },
260         { 0x54a4, 0x01 }, { 0x54a5, 0xe7 }, { 0x54a6, 0x01 }, { 0x54a7, 0xc3 },
261         { 0x54a8, 0x01 }, { 0x54a9, 0x94 }, { 0x54aa, 0x01 }, { 0x54ab, 0x72 },
262         { 0x54ac, 0x01 }, { 0x54ad, 0x57 },
263
264         /* AWB */
265         { OV9740_AWB_CTRL00,            0xf0 },
266         { OV9740_AWB_CTRL01,            0x00 },
267         { OV9740_AWB_CTRL02,            0x41 },
268         { OV9740_AWB_CTRL03,            0x42 },
269         { OV9740_AWB_ADV_CTRL01,        0x8a },
270         { OV9740_AWB_ADV_CTRL02,        0x61 },
271         { OV9740_AWB_ADV_CTRL03,        0xce },
272         { OV9740_AWB_ADV_CTRL04,        0xa8 },
273         { OV9740_AWB_ADV_CTRL05,        0x17 },
274         { OV9740_AWB_ADV_CTRL06,        0x1f },
275         { OV9740_AWB_ADV_CTRL07,        0x27 },
276         { OV9740_AWB_ADV_CTRL08,        0x41 },
277         { OV9740_AWB_ADV_CTRL09,        0x34 },
278         { OV9740_AWB_ADV_CTRL10,        0xf0 },
279         { OV9740_AWB_ADV_CTRL11,        0x10 },
280         { OV9740_AWB_CTRL0F,            0xff },
281         { OV9740_AWB_CTRL10,            0x00 },
282         { OV9740_AWB_CTRL11,            0xff },
283         { OV9740_AWB_CTRL12,            0x00 },
284         { OV9740_AWB_CTRL13,            0xff },
285         { OV9740_AWB_CTRL14,            0x00 },
286
287         /* CIP */
288         { 0x530d, 0x12 },
289
290         /* CMX */
291         { 0x5380, 0x01 }, { 0x5381, 0x00 }, { 0x5382, 0x00 }, { 0x5383, 0x17 },
292         { 0x5384, 0x00 }, { 0x5385, 0x01 }, { 0x5386, 0x00 }, { 0x5387, 0x00 },
293         { 0x5388, 0x00 }, { 0x5389, 0xe0 }, { 0x538a, 0x00 }, { 0x538b, 0x20 },
294         { 0x538c, 0x00 }, { 0x538d, 0x00 }, { 0x538e, 0x00 }, { 0x538f, 0x16 },
295         { 0x5390, 0x00 }, { 0x5391, 0x9c }, { 0x5392, 0x00 }, { 0x5393, 0xa0 },
296         { 0x5394, 0x18 },
297
298         /* 50/60 Detection */
299         { 0x3c0a, 0x9c }, { 0x3c0b, 0x3f },
300
301         /* Output Select */
302         { OV9740_IO_OUTPUT_SEL01,       0x00 },
303         { OV9740_IO_OUTPUT_SEL02,       0x00 },
304         { OV9740_IO_CREL00,             0x00 },
305         { OV9740_IO_CREL01,             0x00 },
306         { OV9740_IO_CREL02,             0x00 },
307
308         /* AWB Control */
309         { OV9740_AWB_MANUAL_CTRL,       0x00 },
310
311         /* Analog Control */
312         { OV9740_ANALOG_CTRL03,         0xaa },
313         { OV9740_ANALOG_CTRL32,         0x2f },
314         { OV9740_ANALOG_CTRL20,         0x66 },
315         { OV9740_ANALOG_CTRL21,         0xc0 },
316         { OV9740_ANALOG_CTRL31,         0x52 },
317         { OV9740_ANALOG_CTRL33,         0x50 },
318         { OV9740_ANALOG_CTRL30,         0xca },
319         { OV9740_ANALOG_CTRL04,         0x0c },
320         { OV9740_ANALOG_CTRL01,         0x40 },
321         { OV9740_ANALOG_CTRL02,         0x16 },
322         { OV9740_ANALOG_CTRL10,         0xa1 },
323         { OV9740_ANALOG_CTRL12,         0x24 },
324         { OV9740_ANALOG_CTRL22,         0x9f },
325         { OV9740_ANALOG_CTRL15,         0xf0 },
326
327         /* Sensor Control */
328         { OV9740_SENSOR_CTRL03,         0x42 },
329         { OV9740_SENSOR_CTRL04,         0x10 },
330         { OV9740_SENSOR_CTRL05,         0x45 },
331         { OV9740_SENSOR_CTRL07,         0x14 },
332
333         /* Timing Control */
334         { OV9740_TIMING_CTRL33,         0x04 },
335         { OV9740_TIMING_CTRL35,         0x02 },
336         { OV9740_TIMING_CTRL19,         0x6e },
337         { OV9740_TIMING_CTRL17,         0x94 },
338
339         /* AEC/AGC Control */
340         { OV9740_AEC_ENABLE,            0x10 },
341         { OV9740_GAIN_CEILING_01,       0x00 },
342         { OV9740_GAIN_CEILING_02,       0x7f },
343         { OV9740_AEC_HI_THRESHOLD,      0xa0 },
344         { OV9740_AEC_3A1A,              0x05 },
345         { OV9740_AEC_CTRL1B_WPT2,       0x50 },
346         { OV9740_AEC_CTRL0F_WPT,        0x50 },
347         { OV9740_AEC_CTRL10_BPT,        0x4c },
348         { OV9740_AEC_CTRL1E_BPT2,       0x4c },
349         { OV9740_AEC_LO_THRESHOLD,      0x26 },
350
351         /* BLC Control */
352         { OV9740_BLC_AUTO_ENABLE,       0x45 },
353         { OV9740_BLC_MODE,              0x18 },
354
355         /* DVP Control */
356         { OV9740_DVP_VSYNC_CTRL02,      0x04 },
357         { OV9740_DVP_VSYNC_MODE,        0x00 },
358         { OV9740_DVP_VSYNC_CTRL06,      0x08 },
359
360         /* PLL Setting */
361         { OV9740_PLL_MODE_CTRL01,       0x20 },
362         { OV9740_PRE_PLL_CLK_DIV,       0x03 },
363         { OV9740_PLL_MULTIPLIER,        0x4c },
364         { OV9740_VT_SYS_CLK_DIV,        0x01 },
365         { OV9740_VT_PIX_CLK_DIV,        0x08 },
366         { OV9740_PLL_CTRL3010,          0x01 },
367         { OV9740_VFIFO_CTRL00,          0x82 },
368
369         /* Timing Setting */
370         /* VTS */
371         { OV9740_FRM_LENGTH_LN_HI,      0x03 },
372         { OV9740_FRM_LENGTH_LN_LO,      0x07 },
373         /* HTS */
374         { OV9740_LN_LENGTH_PCK_HI,      0x06 },
375         { OV9740_LN_LENGTH_PCK_LO,      0x62 },
376
377         /* MIPI Control */
378         { OV9740_MIPI_CTRL00,           0x44 }, /* 0x64 for discontinuous clk */
379         { OV9740_MIPI_3837,             0x01 },
380         { OV9740_MIPI_CTRL01,           0x0f },
381         { OV9740_MIPI_CTRL03,           0x05 },
382         { OV9740_MIPI_CTRL05,           0x10 },
383         { OV9740_VFIFO_RD_CTRL,         0x16 },
384         { OV9740_MIPI_CTRL_3012,        0x70 },
385         { OV9740_SC_CMMM_MIPI_CTR,      0x01 },
386
387         /* YUYV order */
388         { OV9740_ISP_CTRL19,            0x02 },
389 };
390
391 static enum v4l2_mbus_pixelcode ov9740_codes[] = {
392         V4L2_MBUS_FMT_YUYV8_2X8,
393 };
394
395 static const struct v4l2_queryctrl ov9740_controls[] = {
396         {
397                 .id             = V4L2_CID_VFLIP,
398                 .type           = V4L2_CTRL_TYPE_BOOLEAN,
399                 .name           = "Flip Vertically",
400                 .minimum        = 0,
401                 .maximum        = 1,
402                 .step           = 1,
403                 .default_value  = 0,
404         },
405         {
406                 .id             = V4L2_CID_HFLIP,
407                 .type           = V4L2_CTRL_TYPE_BOOLEAN,
408                 .name           = "Flip Horizontally",
409                 .minimum        = 0,
410                 .maximum        = 1,
411                 .step           = 1,
412                 .default_value  = 0,
413         },
414 };
415
416 /* read a register */
417 static int ov9740_reg_read(struct i2c_client *client, u16 reg, u8 *val)
418 {
419         int ret;
420         struct i2c_msg msg[] = {
421                 {
422                         .addr   = client->addr,
423                         .flags  = 0,
424                         .len    = 2,
425                         .buf    = (u8 *)&reg,
426                 },
427                 {
428                         .addr   = client->addr,
429                         .flags  = I2C_M_RD,
430                         .len    = 1,
431                         .buf    = val,
432                 },
433         };
434
435         reg = swab16(reg);
436
437         ret = i2c_transfer(client->adapter, msg, 2);
438         if (ret < 0) {
439                 dev_err(&client->dev, "Failed reading register 0x%04x!\n", reg);
440                 return ret;
441         }
442
443         return 0;
444 }
445
446 /* write a register */
447 static int ov9740_reg_write(struct i2c_client *client, u16 reg, u8 val)
448 {
449         struct i2c_msg msg;
450         struct {
451                 u16 reg;
452                 u8 val;
453         } __packed buf;
454         int ret;
455
456         reg = swab16(reg);
457
458         buf.reg = reg;
459         buf.val = val;
460
461         msg.addr        = client->addr;
462         msg.flags       = 0;
463         msg.len         = 3;
464         msg.buf         = (u8 *)&buf;
465
466         ret = i2c_transfer(client->adapter, &msg, 1);
467         if (ret < 0) {
468                 dev_err(&client->dev, "Failed writing register 0x%04x!\n", reg);
469                 return ret;
470         }
471
472         return 0;
473 }
474
475
476 /* Read a register, alter its bits, write it back */
477 static int ov9740_reg_rmw(struct i2c_client *client, u16 reg, u8 set, u8 unset)
478 {
479         u8 val;
480         int ret;
481
482         ret = ov9740_reg_read(client, reg, &val);
483         if (ret < 0) {
484                 dev_err(&client->dev,
485                         "[Read]-Modify-Write of register 0x%04x failed!\n",
486                         reg);
487                 return ret;
488         }
489
490         val |= set;
491         val &= ~unset;
492
493         ret = ov9740_reg_write(client, reg, val);
494         if (ret < 0) {
495                 dev_err(&client->dev,
496                         "Read-Modify-[Write] of register 0x%04x failed!\n",
497                         reg);
498                 return ret;
499         }
500
501         return 0;
502 }
503
504 static int ov9740_reg_write_array(struct i2c_client *client,
505                                   const struct ov9740_reg *regarray,
506                                   int regarraylen)
507 {
508         int i;
509         int ret;
510
511         for (i = 0; i < regarraylen; i++) {
512                 ret = ov9740_reg_write(client,
513                                        regarray[i].reg, regarray[i].val);
514                 if (ret < 0)
515                         return ret;
516         }
517
518         return 0;
519 }
520
521 /* Start/Stop streaming from the device */
522 static int ov9740_s_stream(struct v4l2_subdev *sd, int enable)
523 {
524         struct i2c_client *client = v4l2_get_subdevdata(sd);
525         struct ov9740_priv *priv = to_ov9740(sd);
526         int ret;
527
528         /* Program orientation register. */
529         if (priv->flag_vflip)
530                 ret = ov9740_reg_rmw(client, OV9740_IMAGE_ORT, 0x2, 0);
531         else
532                 ret = ov9740_reg_rmw(client, OV9740_IMAGE_ORT, 0, 0x2);
533         if (ret < 0)
534                 return ret;
535
536         if (priv->flag_hflip)
537                 ret = ov9740_reg_rmw(client, OV9740_IMAGE_ORT, 0x1, 0);
538         else
539                 ret = ov9740_reg_rmw(client, OV9740_IMAGE_ORT, 0, 0x1);
540         if (ret < 0)
541                 return ret;
542
543         if (enable) {
544                 dev_dbg(&client->dev, "Enabling Streaming\n");
545                 /* Start Streaming */
546                 ret = ov9740_reg_write(client, OV9740_MODE_SELECT, 0x01);
547
548         } else {
549                 dev_dbg(&client->dev, "Disabling Streaming\n");
550                 /* Software Reset */
551                 ret = ov9740_reg_write(client, OV9740_SOFTWARE_RESET, 0x01);
552                 if (!ret)
553                         /* Setting Streaming to Standby */
554                         ret = ov9740_reg_write(client, OV9740_MODE_SELECT,
555                                                0x00);
556         }
557
558         priv->current_enable = enable;
559
560         return ret;
561 }
562
563 /* Alter bus settings on camera side */
564 static int ov9740_set_bus_param(struct soc_camera_device *icd,
565                                 unsigned long flags)
566 {
567         return 0;
568 }
569
570 /* Request bus settings on camera side */
571 static unsigned long ov9740_query_bus_param(struct soc_camera_device *icd)
572 {
573         struct soc_camera_link *icl = to_soc_camera_link(icd);
574
575         unsigned long flags = SOCAM_PCLK_SAMPLE_RISING | SOCAM_MASTER |
576                 SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_HSYNC_ACTIVE_HIGH |
577                 SOCAM_DATA_ACTIVE_HIGH | SOCAM_DATAWIDTH_8;
578
579         return soc_camera_apply_sensor_flags(icl, flags);
580 }
581
582 /* select nearest higher resolution for capture */
583 static void ov9740_res_roundup(u32 *width, u32 *height)
584 {
585         /* Width must be a multiple of 4 pixels. */
586         *width = ALIGN(*width, 4);
587
588         /* Max resolution is 1280x720 (720p). */
589         if (*width > OV9740_MAX_WIDTH)
590                 *width = OV9740_MAX_WIDTH;
591
592         if (*height > OV9740_MAX_HEIGHT)
593                 *height = OV9740_MAX_HEIGHT;
594 }
595
596 /* Setup registers according to resolution and color encoding */
597 static int ov9740_set_res(struct i2c_client *client, u32 width, u32 height)
598 {
599         u32 x_start;
600         u32 y_start;
601         u32 x_end;
602         u32 y_end;
603         bool scaling = 0;
604         u32 scale_input_x;
605         u32 scale_input_y;
606         int ret;
607
608         if ((width != OV9740_MAX_WIDTH) || (height != OV9740_MAX_HEIGHT))
609                 scaling = 1;
610
611         /*
612          * Try to use as much of the sensor area as possible when supporting
613          * smaller resolutions.  Depending on the aspect ratio of the
614          * chosen resolution, we can either use the full width of the sensor,
615          * or the full height of the sensor (or both if the aspect ratio is
616          * the same as 1280x720.
617          */
618         if ((OV9740_MAX_WIDTH * height) > (OV9740_MAX_HEIGHT * width)) {
619                 scale_input_x = (OV9740_MAX_HEIGHT * width) / height;
620                 scale_input_y = OV9740_MAX_HEIGHT;
621         } else {
622                 scale_input_x = OV9740_MAX_WIDTH;
623                 scale_input_y = (OV9740_MAX_WIDTH * height) / width;
624         }
625
626         /* These describe the area of the sensor to use. */
627         x_start = (OV9740_MAX_WIDTH - scale_input_x) / 2;
628         y_start = (OV9740_MAX_HEIGHT - scale_input_y) / 2;
629         x_end = x_start + scale_input_x - 1;
630         y_end = y_start + scale_input_y - 1;
631
632         ret = ov9740_reg_write(client, OV9740_X_ADDR_START_HI, x_start >> 8);
633         if (ret)
634                 goto done;
635         ret = ov9740_reg_write(client, OV9740_X_ADDR_START_LO, x_start & 0xff);
636         if (ret)
637                 goto done;
638         ret = ov9740_reg_write(client, OV9740_Y_ADDR_START_HI, y_start >> 8);
639         if (ret)
640                 goto done;
641         ret = ov9740_reg_write(client, OV9740_Y_ADDR_START_LO, y_start & 0xff);
642         if (ret)
643                 goto done;
644
645         ret = ov9740_reg_write(client, OV9740_X_ADDR_END_HI, x_end >> 8);
646         if (ret)
647                 goto done;
648         ret = ov9740_reg_write(client, OV9740_X_ADDR_END_LO, x_end & 0xff);
649         if (ret)
650                 goto done;
651         ret = ov9740_reg_write(client, OV9740_Y_ADDR_END_HI, y_end >> 8);
652         if (ret)
653                 goto done;
654         ret = ov9740_reg_write(client, OV9740_Y_ADDR_END_LO, y_end & 0xff);
655         if (ret)
656                 goto done;
657
658         ret = ov9740_reg_write(client, OV9740_X_OUTPUT_SIZE_HI, width >> 8);
659         if (ret)
660                 goto done;
661         ret = ov9740_reg_write(client, OV9740_X_OUTPUT_SIZE_LO, width & 0xff);
662         if (ret)
663                 goto done;
664         ret = ov9740_reg_write(client, OV9740_Y_OUTPUT_SIZE_HI, height >> 8);
665         if (ret)
666                 goto done;
667         ret = ov9740_reg_write(client, OV9740_Y_OUTPUT_SIZE_LO, height & 0xff);
668         if (ret)
669                 goto done;
670
671         ret = ov9740_reg_write(client, OV9740_ISP_CTRL1E, scale_input_x >> 8);
672         if (ret)
673                 goto done;
674         ret = ov9740_reg_write(client, OV9740_ISP_CTRL1F, scale_input_x & 0xff);
675         if (ret)
676                 goto done;
677         ret = ov9740_reg_write(client, OV9740_ISP_CTRL20, scale_input_y >> 8);
678         if (ret)
679                 goto done;
680         ret = ov9740_reg_write(client, OV9740_ISP_CTRL21, scale_input_y & 0xff);
681         if (ret)
682                 goto done;
683
684         ret = ov9740_reg_write(client, OV9740_VFIFO_READ_START_HI,
685                                (scale_input_x - width) >> 8);
686         if (ret)
687                 goto done;
688         ret = ov9740_reg_write(client, OV9740_VFIFO_READ_START_LO,
689                                (scale_input_x - width) & 0xff);
690         if (ret)
691                 goto done;
692
693         ret = ov9740_reg_write(client, OV9740_ISP_CTRL00, 0xff);
694         if (ret)
695                 goto done;
696         ret = ov9740_reg_write(client, OV9740_ISP_CTRL01, 0xef |
697                                                           (scaling << 4));
698         if (ret)
699                 goto done;
700         ret = ov9740_reg_write(client, OV9740_ISP_CTRL03, 0xff);
701
702 done:
703         return ret;
704 }
705
706 /* set the format we will capture in */
707 static int ov9740_s_fmt(struct v4l2_subdev *sd,
708                         struct v4l2_mbus_framefmt *mf)
709 {
710         struct i2c_client *client = v4l2_get_subdevdata(sd);
711         struct ov9740_priv *priv = to_ov9740(sd);
712         enum v4l2_colorspace cspace;
713         enum v4l2_mbus_pixelcode code = mf->code;
714         int ret;
715
716         ov9740_res_roundup(&mf->width, &mf->height);
717
718         switch (code) {
719         case V4L2_MBUS_FMT_YUYV8_2X8:
720                 cspace = V4L2_COLORSPACE_SRGB;
721                 break;
722         default:
723                 return -EINVAL;
724         }
725
726         ret = ov9740_reg_write_array(client, ov9740_defaults,
727                                      ARRAY_SIZE(ov9740_defaults));
728         if (ret < 0)
729                 return ret;
730
731         ret = ov9740_set_res(client, mf->width, mf->height);
732         if (ret < 0)
733                 return ret;
734
735         mf->code        = code;
736         mf->colorspace  = cspace;
737
738         memcpy(&priv->current_mf, mf, sizeof(struct v4l2_mbus_framefmt));
739
740         return ret;
741 }
742
743 static int ov9740_try_fmt(struct v4l2_subdev *sd,
744                           struct v4l2_mbus_framefmt *mf)
745 {
746         ov9740_res_roundup(&mf->width, &mf->height);
747
748         mf->field = V4L2_FIELD_NONE;
749         mf->code = V4L2_MBUS_FMT_YUYV8_2X8;
750         mf->colorspace = V4L2_COLORSPACE_SRGB;
751
752         return 0;
753 }
754
755 static int ov9740_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
756                            enum v4l2_mbus_pixelcode *code)
757 {
758         if (index >= ARRAY_SIZE(ov9740_codes))
759                 return -EINVAL;
760
761         *code = ov9740_codes[index];
762
763         return 0;
764 }
765
766 static int ov9740_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
767 {
768         a->bounds.left          = 0;
769         a->bounds.top           = 0;
770         a->bounds.width         = OV9740_MAX_WIDTH;
771         a->bounds.height        = OV9740_MAX_HEIGHT;
772         a->defrect              = a->bounds;
773         a->type                 = V4L2_BUF_TYPE_VIDEO_CAPTURE;
774         a->pixelaspect.numerator        = 1;
775         a->pixelaspect.denominator      = 1;
776
777         return 0;
778 }
779
780 static int ov9740_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
781 {
782         a->c.left               = 0;
783         a->c.top                = 0;
784         a->c.width              = OV9740_MAX_WIDTH;
785         a->c.height             = OV9740_MAX_HEIGHT;
786         a->type                 = V4L2_BUF_TYPE_VIDEO_CAPTURE;
787
788         return 0;
789 }
790
791 /* Get status of additional camera capabilities */
792 static int ov9740_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
793 {
794         struct ov9740_priv *priv = to_ov9740(sd);
795
796         switch (ctrl->id) {
797         case V4L2_CID_VFLIP:
798                 ctrl->value = priv->flag_vflip;
799                 break;
800         case V4L2_CID_HFLIP:
801                 ctrl->value = priv->flag_hflip;
802                 break;
803         default:
804                 return -EINVAL;
805         }
806
807         return 0;
808 }
809
810 /* Set status of additional camera capabilities */
811 static int ov9740_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
812 {
813         struct ov9740_priv *priv = to_ov9740(sd);
814
815         switch (ctrl->id) {
816         case V4L2_CID_VFLIP:
817                 priv->flag_vflip = ctrl->value;
818                 break;
819         case V4L2_CID_HFLIP:
820                 priv->flag_hflip = ctrl->value;
821                 break;
822         default:
823                 return -EINVAL;
824         }
825
826         return 0;
827 }
828
829 /* Get chip identification */
830 static int ov9740_g_chip_ident(struct v4l2_subdev *sd,
831                                struct v4l2_dbg_chip_ident *id)
832 {
833         struct ov9740_priv *priv = to_ov9740(sd);
834
835         id->ident = priv->ident;
836         id->revision = priv->revision;
837
838         return 0;
839 }
840
841 static int ov9740_s_power(struct v4l2_subdev *sd, int on)
842 {
843         struct ov9740_priv *priv = to_ov9740(sd);
844
845         if (!priv->current_enable)
846                 return 0;
847
848         if (on) {
849                 ov9740_s_fmt(sd, &priv->current_mf);
850                 ov9740_s_stream(sd, priv->current_enable);
851         } else {
852                 ov9740_s_stream(sd, 0);
853                 priv->current_enable = true;
854         }
855
856         return 0;
857 }
858
859 #ifdef CONFIG_VIDEO_ADV_DEBUG
860 static int ov9740_get_register(struct v4l2_subdev *sd,
861                                struct v4l2_dbg_register *reg)
862 {
863         struct i2c_client *client = v4l2_get_subdevdata(sd);
864         int ret;
865         u8 val;
866
867         if (reg->reg & ~0xffff)
868                 return -EINVAL;
869
870         reg->size = 2;
871
872         ret = ov9740_reg_read(client, reg->reg, &val);
873         if (ret)
874                 return ret;
875
876         reg->val = (__u64)val;
877
878         return ret;
879 }
880
881 static int ov9740_set_register(struct v4l2_subdev *sd,
882                                struct v4l2_dbg_register *reg)
883 {
884         struct i2c_client *client = v4l2_get_subdevdata(sd);
885
886         if (reg->reg & ~0xffff || reg->val & ~0xff)
887                 return -EINVAL;
888
889         return ov9740_reg_write(client, reg->reg, reg->val);
890 }
891 #endif
892
893 static int ov9740_video_probe(struct soc_camera_device *icd,
894                               struct i2c_client *client)
895 {
896         struct v4l2_subdev *sd = i2c_get_clientdata(client);
897         struct ov9740_priv *priv = to_ov9740(sd);
898         u8 modelhi, modello;
899         int ret;
900
901         /* We must have a parent by now. And it cannot be a wrong one. */
902         BUG_ON(!icd->parent ||
903                to_soc_camera_host(icd->parent)->nr != icd->iface);
904
905         /*
906          * check and show product ID and manufacturer ID
907          */
908         ret = ov9740_reg_read(client, OV9740_MODEL_ID_HI, &modelhi);
909         if (ret < 0)
910                 goto err;
911
912         ret = ov9740_reg_read(client, OV9740_MODEL_ID_LO, &modello);
913         if (ret < 0)
914                 goto err;
915
916         priv->model = (modelhi << 8) | modello;
917
918         ret = ov9740_reg_read(client, OV9740_REVISION_NUMBER, &priv->revision);
919         if (ret < 0)
920                 goto err;
921
922         ret = ov9740_reg_read(client, OV9740_MANUFACTURER_ID, &priv->manid);
923         if (ret < 0)
924                 goto err;
925
926         ret = ov9740_reg_read(client, OV9740_SMIA_VERSION, &priv->smiaver);
927         if (ret < 0)
928                 goto err;
929
930         if (priv->model != 0x9740) {
931                 ret = -ENODEV;
932                 goto err;
933         }
934
935         priv->ident = V4L2_IDENT_OV9740;
936
937         dev_info(&client->dev, "ov9740 Model ID 0x%04x, Revision 0x%02x, "
938                  "Manufacturer 0x%02x, SMIA Version 0x%02x\n",
939                  priv->model, priv->revision, priv->manid, priv->smiaver);
940
941 err:
942         return ret;
943 }
944
945 static struct soc_camera_ops ov9740_ops = {
946         .set_bus_param          = ov9740_set_bus_param,
947         .query_bus_param        = ov9740_query_bus_param,
948         .controls               = ov9740_controls,
949         .num_controls           = ARRAY_SIZE(ov9740_controls),
950 };
951
952 static struct v4l2_subdev_video_ops ov9740_video_ops = {
953         .s_stream               = ov9740_s_stream,
954         .s_mbus_fmt             = ov9740_s_fmt,
955         .try_mbus_fmt           = ov9740_try_fmt,
956         .enum_mbus_fmt          = ov9740_enum_fmt,
957         .cropcap                = ov9740_cropcap,
958         .g_crop                 = ov9740_g_crop,
959 };
960
961 static struct v4l2_subdev_core_ops ov9740_core_ops = {
962         .g_ctrl                 = ov9740_g_ctrl,
963         .s_ctrl                 = ov9740_s_ctrl,
964         .g_chip_ident           = ov9740_g_chip_ident,
965         .s_power                = ov9740_s_power,
966 #ifdef CONFIG_VIDEO_ADV_DEBUG
967         .g_register             = ov9740_get_register,
968         .s_register             = ov9740_set_register,
969 #endif
970 };
971
972 static struct v4l2_subdev_ops ov9740_subdev_ops = {
973         .core                   = &ov9740_core_ops,
974         .video                  = &ov9740_video_ops,
975 };
976
977 /*
978  * i2c_driver function
979  */
980 static int ov9740_probe(struct i2c_client *client,
981                         const struct i2c_device_id *did)
982 {
983         struct ov9740_priv *priv;
984         struct soc_camera_device *icd   = client->dev.platform_data;
985         struct soc_camera_link *icl;
986         int ret;
987
988         if (!icd) {
989                 dev_err(&client->dev, "Missing soc-camera data!\n");
990                 return -EINVAL;
991         }
992
993         icl = to_soc_camera_link(icd);
994         if (!icl) {
995                 dev_err(&client->dev, "Missing platform_data for driver\n");
996                 return -EINVAL;
997         }
998
999         priv = kzalloc(sizeof(struct ov9740_priv), GFP_KERNEL);
1000         if (!priv) {
1001                 dev_err(&client->dev, "Failed to allocate private data!\n");
1002                 return -ENOMEM;
1003         }
1004
1005         v4l2_i2c_subdev_init(&priv->subdev, client, &ov9740_subdev_ops);
1006
1007         icd->ops = &ov9740_ops;
1008
1009         ret = ov9740_video_probe(icd, client);
1010         if (ret < 0) {
1011                 icd->ops = NULL;
1012                 kfree(priv);
1013         }
1014
1015         return ret;
1016 }
1017
1018 static int ov9740_remove(struct i2c_client *client)
1019 {
1020         struct ov9740_priv *priv = i2c_get_clientdata(client);
1021
1022         kfree(priv);
1023
1024         return 0;
1025 }
1026
1027 static const struct i2c_device_id ov9740_id[] = {
1028         { "ov9740", 0 },
1029         { }
1030 };
1031 MODULE_DEVICE_TABLE(i2c, ov9740_id);
1032
1033 static struct i2c_driver ov9740_i2c_driver = {
1034         .driver = {
1035                 .name = "ov9740",
1036         },
1037         .probe    = ov9740_probe,
1038         .remove   = ov9740_remove,
1039         .id_table = ov9740_id,
1040 };
1041
1042 static int __init ov9740_module_init(void)
1043 {
1044         return i2c_add_driver(&ov9740_i2c_driver);
1045 }
1046
1047 static void __exit ov9740_module_exit(void)
1048 {
1049         i2c_del_driver(&ov9740_i2c_driver);
1050 }
1051
1052 module_init(ov9740_module_init);
1053 module_exit(ov9740_module_exit);
1054
1055 MODULE_DESCRIPTION("SoC Camera driver for OmniVision OV9740");
1056 MODULE_AUTHOR("Andrew Chew <achew@nvidia.com>");
1057 MODULE_LICENSE("GPL v2");