diff options
Diffstat (limited to 'drivers/media/video/mt9v011.c')
-rw-r--r-- | drivers/media/video/mt9v011.c | 722 |
1 files changed, 722 insertions, 0 deletions
diff --git a/drivers/media/video/mt9v011.c b/drivers/media/video/mt9v011.c new file mode 100644 index 00000000000..893a8b8f514 --- /dev/null +++ b/drivers/media/video/mt9v011.c | |||
@@ -0,0 +1,722 @@ | |||
1 | /* | ||
2 | * mt9v011 -Micron 1/4-Inch VGA Digital Image Sensor | ||
3 | * | ||
4 | * Copyright (c) 2009 Mauro Carvalho Chehab (mchehab@redhat.com) | ||
5 | * This code is placed under the terms of the GNU General Public License v2 | ||
6 | */ | ||
7 | |||
8 | #include <linux/i2c.h> | ||
9 | #include <linux/slab.h> | ||
10 | #include <linux/videodev2.h> | ||
11 | #include <linux/delay.h> | ||
12 | #include <asm/div64.h> | ||
13 | #include <media/v4l2-device.h> | ||
14 | #include <media/v4l2-chip-ident.h> | ||
15 | #include <media/mt9v011.h> | ||
16 | |||
17 | MODULE_DESCRIPTION("Micron mt9v011 sensor driver"); | ||
18 | MODULE_AUTHOR("Mauro Carvalho Chehab <mchehab@redhat.com>"); | ||
19 | MODULE_LICENSE("GPL"); | ||
20 | |||
21 | static int debug; | ||
22 | module_param(debug, int, 0); | ||
23 | MODULE_PARM_DESC(debug, "Debug level (0-2)"); | ||
24 | |||
25 | #define R00_MT9V011_CHIP_VERSION 0x00 | ||
26 | #define R01_MT9V011_ROWSTART 0x01 | ||
27 | #define R02_MT9V011_COLSTART 0x02 | ||
28 | #define R03_MT9V011_HEIGHT 0x03 | ||
29 | #define R04_MT9V011_WIDTH 0x04 | ||
30 | #define R05_MT9V011_HBLANK 0x05 | ||
31 | #define R06_MT9V011_VBLANK 0x06 | ||
32 | #define R07_MT9V011_OUT_CTRL 0x07 | ||
33 | #define R09_MT9V011_SHUTTER_WIDTH 0x09 | ||
34 | #define R0A_MT9V011_CLK_SPEED 0x0a | ||
35 | #define R0B_MT9V011_RESTART 0x0b | ||
36 | #define R0C_MT9V011_SHUTTER_DELAY 0x0c | ||
37 | #define R0D_MT9V011_RESET 0x0d | ||
38 | #define R1E_MT9V011_DIGITAL_ZOOM 0x1e | ||
39 | #define R20_MT9V011_READ_MODE 0x20 | ||
40 | #define R2B_MT9V011_GREEN_1_GAIN 0x2b | ||
41 | #define R2C_MT9V011_BLUE_GAIN 0x2c | ||
42 | #define R2D_MT9V011_RED_GAIN 0x2d | ||
43 | #define R2E_MT9V011_GREEN_2_GAIN 0x2e | ||
44 | #define R35_MT9V011_GLOBAL_GAIN 0x35 | ||
45 | #define RF1_MT9V011_CHIP_ENABLE 0xf1 | ||
46 | |||
47 | #define MT9V011_VERSION 0x8232 | ||
48 | #define MT9V011_REV_B_VERSION 0x8243 | ||
49 | |||
50 | /* supported controls */ | ||
51 | static struct v4l2_queryctrl mt9v011_qctrl[] = { | ||
52 | { | ||
53 | .id = V4L2_CID_GAIN, | ||
54 | .type = V4L2_CTRL_TYPE_INTEGER, | ||
55 | .name = "Gain", | ||
56 | .minimum = 0, | ||
57 | .maximum = (1 << 12) - 1 - 0x0020, | ||
58 | .step = 1, | ||
59 | .default_value = 0x0020, | ||
60 | .flags = 0, | ||
61 | }, { | ||
62 | .id = V4L2_CID_EXPOSURE, | ||
63 | .type = V4L2_CTRL_TYPE_INTEGER, | ||
64 | .name = "Exposure", | ||
65 | .minimum = 0, | ||
66 | .maximum = 2047, | ||
67 | .step = 1, | ||
68 | .default_value = 0x01fc, | ||
69 | .flags = 0, | ||
70 | }, { | ||
71 | .id = V4L2_CID_RED_BALANCE, | ||
72 | .type = V4L2_CTRL_TYPE_INTEGER, | ||
73 | .name = "Red Balance", | ||
74 | .minimum = -1 << 9, | ||
75 | .maximum = (1 << 9) - 1, | ||
76 | .step = 1, | ||
77 | .default_value = 0, | ||
78 | .flags = 0, | ||
79 | }, { | ||
80 | .id = V4L2_CID_BLUE_BALANCE, | ||
81 | .type = V4L2_CTRL_TYPE_INTEGER, | ||
82 | .name = "Blue Balance", | ||
83 | .minimum = -1 << 9, | ||
84 | .maximum = (1 << 9) - 1, | ||
85 | .step = 1, | ||
86 | .default_value = 0, | ||
87 | .flags = 0, | ||
88 | }, { | ||
89 | .id = V4L2_CID_HFLIP, | ||
90 | .type = V4L2_CTRL_TYPE_BOOLEAN, | ||
91 | .name = "Mirror", | ||
92 | .minimum = 0, | ||
93 | .maximum = 1, | ||
94 | .step = 1, | ||
95 | .default_value = 0, | ||
96 | .flags = 0, | ||
97 | }, { | ||
98 | .id = V4L2_CID_VFLIP, | ||
99 | .type = V4L2_CTRL_TYPE_BOOLEAN, | ||
100 | .name = "Vflip", | ||
101 | .minimum = 0, | ||
102 | .maximum = 1, | ||
103 | .step = 1, | ||
104 | .default_value = 0, | ||
105 | .flags = 0, | ||
106 | }, { | ||
107 | } | ||
108 | }; | ||
109 | |||
110 | struct mt9v011 { | ||
111 | struct v4l2_subdev sd; | ||
112 | unsigned width, height; | ||
113 | unsigned xtal; | ||
114 | unsigned hflip:1; | ||
115 | unsigned vflip:1; | ||
116 | |||
117 | u16 global_gain, exposure; | ||
118 | s16 red_bal, blue_bal; | ||
119 | }; | ||
120 | |||
121 | static inline struct mt9v011 *to_mt9v011(struct v4l2_subdev *sd) | ||
122 | { | ||
123 | return container_of(sd, struct mt9v011, sd); | ||
124 | } | ||
125 | |||
126 | static int mt9v011_read(struct v4l2_subdev *sd, unsigned char addr) | ||
127 | { | ||
128 | struct i2c_client *c = v4l2_get_subdevdata(sd); | ||
129 | __be16 buffer; | ||
130 | int rc, val; | ||
131 | |||
132 | rc = i2c_master_send(c, &addr, 1); | ||
133 | if (rc != 1) | ||
134 | v4l2_dbg(0, debug, sd, | ||
135 | "i2c i/o error: rc == %d (should be 1)\n", rc); | ||
136 | |||
137 | msleep(10); | ||
138 | |||
139 | rc = i2c_master_recv(c, (char *)&buffer, 2); | ||
140 | if (rc != 2) | ||
141 | v4l2_dbg(0, debug, sd, | ||
142 | "i2c i/o error: rc == %d (should be 2)\n", rc); | ||
143 | |||
144 | val = be16_to_cpu(buffer); | ||
145 | |||
146 | v4l2_dbg(2, debug, sd, "mt9v011: read 0x%02x = 0x%04x\n", addr, val); | ||
147 | |||
148 | return val; | ||
149 | } | ||
150 | |||
151 | static void mt9v011_write(struct v4l2_subdev *sd, unsigned char addr, | ||
152 | u16 value) | ||
153 | { | ||
154 | struct i2c_client *c = v4l2_get_subdevdata(sd); | ||
155 | unsigned char buffer[3]; | ||
156 | int rc; | ||
157 | |||
158 | buffer[0] = addr; | ||
159 | buffer[1] = value >> 8; | ||
160 | buffer[2] = value & 0xff; | ||
161 | |||
162 | v4l2_dbg(2, debug, sd, | ||
163 | "mt9v011: writing 0x%02x 0x%04x\n", buffer[0], value); | ||
164 | rc = i2c_master_send(c, buffer, 3); | ||
165 | if (rc != 3) | ||
166 | v4l2_dbg(0, debug, sd, | ||
167 | "i2c i/o error: rc == %d (should be 3)\n", rc); | ||
168 | } | ||
169 | |||
170 | |||
171 | struct i2c_reg_value { | ||
172 | unsigned char reg; | ||
173 | u16 value; | ||
174 | }; | ||
175 | |||
176 | /* | ||
177 | * Values used at the original driver | ||
178 | * Some values are marked as Reserved at the datasheet | ||
179 | */ | ||
180 | static const struct i2c_reg_value mt9v011_init_default[] = { | ||
181 | { R0D_MT9V011_RESET, 0x0001 }, | ||
182 | { R0D_MT9V011_RESET, 0x0000 }, | ||
183 | |||
184 | { R0C_MT9V011_SHUTTER_DELAY, 0x0000 }, | ||
185 | { R09_MT9V011_SHUTTER_WIDTH, 0x1fc }, | ||
186 | |||
187 | { R0A_MT9V011_CLK_SPEED, 0x0000 }, | ||
188 | { R1E_MT9V011_DIGITAL_ZOOM, 0x0000 }, | ||
189 | |||
190 | { R07_MT9V011_OUT_CTRL, 0x0002 }, /* chip enable */ | ||
191 | }; | ||
192 | |||
193 | |||
194 | static u16 calc_mt9v011_gain(s16 lineargain) | ||
195 | { | ||
196 | |||
197 | u16 digitalgain = 0; | ||
198 | u16 analogmult = 0; | ||
199 | u16 analoginit = 0; | ||
200 | |||
201 | if (lineargain < 0) | ||
202 | lineargain = 0; | ||
203 | |||
204 | /* recommended minimum */ | ||
205 | lineargain += 0x0020; | ||
206 | |||
207 | if (lineargain > 2047) | ||
208 | lineargain = 2047; | ||
209 | |||
210 | if (lineargain > 1023) { | ||
211 | digitalgain = 3; | ||
212 | analogmult = 3; | ||
213 | analoginit = lineargain / 16; | ||
214 | } else if (lineargain > 511) { | ||
215 | digitalgain = 1; | ||
216 | analogmult = 3; | ||
217 | analoginit = lineargain / 8; | ||
218 | } else if (lineargain > 255) { | ||
219 | analogmult = 3; | ||
220 | analoginit = lineargain / 4; | ||
221 | } else if (lineargain > 127) { | ||
222 | analogmult = 1; | ||
223 | analoginit = lineargain / 2; | ||
224 | } else | ||
225 | analoginit = lineargain; | ||
226 | |||
227 | return analoginit + (analogmult << 7) + (digitalgain << 9); | ||
228 | |||
229 | } | ||
230 | |||
231 | static void set_balance(struct v4l2_subdev *sd) | ||
232 | { | ||
233 | struct mt9v011 *core = to_mt9v011(sd); | ||
234 | u16 green_gain, blue_gain, red_gain; | ||
235 | u16 exposure; | ||
236 | s16 bal; | ||
237 | |||
238 | exposure = core->exposure; | ||
239 | |||
240 | green_gain = calc_mt9v011_gain(core->global_gain); | ||
241 | |||
242 | bal = core->global_gain; | ||
243 | bal += (core->blue_bal * core->global_gain / (1 << 7)); | ||
244 | blue_gain = calc_mt9v011_gain(bal); | ||
245 | |||
246 | bal = core->global_gain; | ||
247 | bal += (core->red_bal * core->global_gain / (1 << 7)); | ||
248 | red_gain = calc_mt9v011_gain(bal); | ||
249 | |||
250 | mt9v011_write(sd, R2B_MT9V011_GREEN_1_GAIN, green_gain); | ||
251 | mt9v011_write(sd, R2E_MT9V011_GREEN_2_GAIN, green_gain); | ||
252 | mt9v011_write(sd, R2C_MT9V011_BLUE_GAIN, blue_gain); | ||
253 | mt9v011_write(sd, R2D_MT9V011_RED_GAIN, red_gain); | ||
254 | mt9v011_write(sd, R09_MT9V011_SHUTTER_WIDTH, exposure); | ||
255 | } | ||
256 | |||
257 | static void calc_fps(struct v4l2_subdev *sd, u32 *numerator, u32 *denominator) | ||
258 | { | ||
259 | struct mt9v011 *core = to_mt9v011(sd); | ||
260 | unsigned height, width, hblank, vblank, speed; | ||
261 | unsigned row_time, t_time; | ||
262 | u64 frames_per_ms; | ||
263 | unsigned tmp; | ||
264 | |||
265 | height = mt9v011_read(sd, R03_MT9V011_HEIGHT); | ||
266 | width = mt9v011_read(sd, R04_MT9V011_WIDTH); | ||
267 | hblank = mt9v011_read(sd, R05_MT9V011_HBLANK); | ||
268 | vblank = mt9v011_read(sd, R06_MT9V011_VBLANK); | ||
269 | speed = mt9v011_read(sd, R0A_MT9V011_CLK_SPEED); | ||
270 | |||
271 | row_time = (width + 113 + hblank) * (speed + 2); | ||
272 | t_time = row_time * (height + vblank + 1); | ||
273 | |||
274 | frames_per_ms = core->xtal * 1000l; | ||
275 | do_div(frames_per_ms, t_time); | ||
276 | tmp = frames_per_ms; | ||
277 | |||
278 | v4l2_dbg(1, debug, sd, "Programmed to %u.%03u fps (%d pixel clcks)\n", | ||
279 | tmp / 1000, tmp % 1000, t_time); | ||
280 | |||
281 | if (numerator && denominator) { | ||
282 | *numerator = 1000; | ||
283 | *denominator = (u32)frames_per_ms; | ||
284 | } | ||
285 | } | ||
286 | |||
287 | static u16 calc_speed(struct v4l2_subdev *sd, u32 numerator, u32 denominator) | ||
288 | { | ||
289 | struct mt9v011 *core = to_mt9v011(sd); | ||
290 | unsigned height, width, hblank, vblank; | ||
291 | unsigned row_time, line_time; | ||
292 | u64 t_time, speed; | ||
293 | |||
294 | /* Avoid bogus calculus */ | ||
295 | if (!numerator || !denominator) | ||
296 | return 0; | ||
297 | |||
298 | height = mt9v011_read(sd, R03_MT9V011_HEIGHT); | ||
299 | width = mt9v011_read(sd, R04_MT9V011_WIDTH); | ||
300 | hblank = mt9v011_read(sd, R05_MT9V011_HBLANK); | ||
301 | vblank = mt9v011_read(sd, R06_MT9V011_VBLANK); | ||
302 | |||
303 | row_time = width + 113 + hblank; | ||
304 | line_time = height + vblank + 1; | ||
305 | |||
306 | t_time = core->xtal * ((u64)numerator); | ||
307 | /* round to the closest value */ | ||
308 | t_time += denominator / 2; | ||
309 | do_div(t_time, denominator); | ||
310 | |||
311 | speed = t_time; | ||
312 | do_div(speed, row_time * line_time); | ||
313 | |||
314 | /* Avoid having a negative value for speed */ | ||
315 | if (speed < 2) | ||
316 | speed = 0; | ||
317 | else | ||
318 | speed -= 2; | ||
319 | |||
320 | /* Avoid speed overflow */ | ||
321 | if (speed > 15) | ||
322 | return 15; | ||
323 | |||
324 | return (u16)speed; | ||
325 | } | ||
326 | |||
327 | static void set_res(struct v4l2_subdev *sd) | ||
328 | { | ||
329 | struct mt9v011 *core = to_mt9v011(sd); | ||
330 | unsigned vstart, hstart; | ||
331 | |||
332 | /* | ||
333 | * The mt9v011 doesn't have scaling. So, in order to select the desired | ||
334 | * resolution, we're cropping at the middle of the sensor. | ||
335 | * hblank and vblank should be adjusted, in order to warrant that | ||
336 | * we'll preserve the line timings for 30 fps, no matter what resolution | ||
337 | * is selected. | ||
338 | * NOTE: datasheet says that width (and height) should be filled with | ||
339 | * width-1. However, this doesn't work, since one pixel per line will | ||
340 | * be missing. | ||
341 | */ | ||
342 | |||
343 | hstart = 20 + (640 - core->width) / 2; | ||
344 | mt9v011_write(sd, R02_MT9V011_COLSTART, hstart); | ||
345 | mt9v011_write(sd, R04_MT9V011_WIDTH, core->width); | ||
346 | mt9v011_write(sd, R05_MT9V011_HBLANK, 771 - core->width); | ||
347 | |||
348 | vstart = 8 + (480 - core->height) / 2; | ||
349 | mt9v011_write(sd, R01_MT9V011_ROWSTART, vstart); | ||
350 | mt9v011_write(sd, R03_MT9V011_HEIGHT, core->height); | ||
351 | mt9v011_write(sd, R06_MT9V011_VBLANK, 508 - core->height); | ||
352 | |||
353 | calc_fps(sd, NULL, NULL); | ||
354 | }; | ||
355 | |||
356 | static void set_read_mode(struct v4l2_subdev *sd) | ||
357 | { | ||
358 | struct mt9v011 *core = to_mt9v011(sd); | ||
359 | unsigned mode = 0x1000; | ||
360 | |||
361 | if (core->hflip) | ||
362 | mode |= 0x4000; | ||
363 | |||
364 | if (core->vflip) | ||
365 | mode |= 0x8000; | ||
366 | |||
367 | mt9v011_write(sd, R20_MT9V011_READ_MODE, mode); | ||
368 | } | ||
369 | |||
370 | static int mt9v011_reset(struct v4l2_subdev *sd, u32 val) | ||
371 | { | ||
372 | int i; | ||
373 | |||
374 | for (i = 0; i < ARRAY_SIZE(mt9v011_init_default); i++) | ||
375 | mt9v011_write(sd, mt9v011_init_default[i].reg, | ||
376 | mt9v011_init_default[i].value); | ||
377 | |||
378 | set_balance(sd); | ||
379 | set_res(sd); | ||
380 | set_read_mode(sd); | ||
381 | |||
382 | return 0; | ||
383 | }; | ||
384 | |||
385 | static int mt9v011_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl) | ||
386 | { | ||
387 | struct mt9v011 *core = to_mt9v011(sd); | ||
388 | |||
389 | v4l2_dbg(1, debug, sd, "g_ctrl called\n"); | ||
390 | |||
391 | switch (ctrl->id) { | ||
392 | case V4L2_CID_GAIN: | ||
393 | ctrl->value = core->global_gain; | ||
394 | return 0; | ||
395 | case V4L2_CID_EXPOSURE: | ||
396 | ctrl->value = core->exposure; | ||
397 | return 0; | ||
398 | case V4L2_CID_RED_BALANCE: | ||
399 | ctrl->value = core->red_bal; | ||
400 | return 0; | ||
401 | case V4L2_CID_BLUE_BALANCE: | ||
402 | ctrl->value = core->blue_bal; | ||
403 | return 0; | ||
404 | case V4L2_CID_HFLIP: | ||
405 | ctrl->value = core->hflip ? 1 : 0; | ||
406 | return 0; | ||
407 | case V4L2_CID_VFLIP: | ||
408 | ctrl->value = core->vflip ? 1 : 0; | ||
409 | return 0; | ||
410 | } | ||
411 | return -EINVAL; | ||
412 | } | ||
413 | |||
414 | static int mt9v011_queryctrl(struct v4l2_subdev *sd, struct v4l2_queryctrl *qc) | ||
415 | { | ||
416 | int i; | ||
417 | |||
418 | v4l2_dbg(1, debug, sd, "queryctrl called\n"); | ||
419 | |||
420 | for (i = 0; i < ARRAY_SIZE(mt9v011_qctrl); i++) | ||
421 | if (qc->id && qc->id == mt9v011_qctrl[i].id) { | ||
422 | memcpy(qc, &(mt9v011_qctrl[i]), | ||
423 | sizeof(*qc)); | ||
424 | return 0; | ||
425 | } | ||
426 | |||
427 | return -EINVAL; | ||
428 | } | ||
429 | |||
430 | |||
431 | static int mt9v011_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl) | ||
432 | { | ||
433 | struct mt9v011 *core = to_mt9v011(sd); | ||
434 | u8 i, n; | ||
435 | n = ARRAY_SIZE(mt9v011_qctrl); | ||
436 | |||
437 | for (i = 0; i < n; i++) { | ||
438 | if (ctrl->id != mt9v011_qctrl[i].id) | ||
439 | continue; | ||
440 | if (ctrl->value < mt9v011_qctrl[i].minimum || | ||
441 | ctrl->value > mt9v011_qctrl[i].maximum) | ||
442 | return -ERANGE; | ||
443 | v4l2_dbg(1, debug, sd, "s_ctrl: id=%d, value=%d\n", | ||
444 | ctrl->id, ctrl->value); | ||
445 | break; | ||
446 | } | ||
447 | |||
448 | switch (ctrl->id) { | ||
449 | case V4L2_CID_GAIN: | ||
450 | core->global_gain = ctrl->value; | ||
451 | break; | ||
452 | case V4L2_CID_EXPOSURE: | ||
453 | core->exposure = ctrl->value; | ||
454 | break; | ||
455 | case V4L2_CID_RED_BALANCE: | ||
456 | core->red_bal = ctrl->value; | ||
457 | break; | ||
458 | case V4L2_CID_BLUE_BALANCE: | ||
459 | core->blue_bal = ctrl->value; | ||
460 | break; | ||
461 | case V4L2_CID_HFLIP: | ||
462 | core->hflip = ctrl->value; | ||
463 | set_read_mode(sd); | ||
464 | return 0; | ||
465 | case V4L2_CID_VFLIP: | ||
466 | core->vflip = ctrl->value; | ||
467 | set_read_mode(sd); | ||
468 | return 0; | ||
469 | default: | ||
470 | return -EINVAL; | ||
471 | } | ||
472 | |||
473 | set_balance(sd); | ||
474 | |||
475 | return 0; | ||
476 | } | ||
477 | |||
478 | static int mt9v011_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned index, | ||
479 | enum v4l2_mbus_pixelcode *code) | ||
480 | { | ||
481 | if (index > 0) | ||
482 | return -EINVAL; | ||
483 | |||
484 | *code = V4L2_MBUS_FMT_SGRBG8_1X8; | ||
485 | return 0; | ||
486 | } | ||
487 | |||
488 | static int mt9v011_try_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *fmt) | ||
489 | { | ||
490 | if (fmt->code != V4L2_MBUS_FMT_SGRBG8_1X8) | ||
491 | return -EINVAL; | ||
492 | |||
493 | v4l_bound_align_image(&fmt->width, 48, 639, 1, | ||
494 | &fmt->height, 32, 480, 1, 0); | ||
495 | fmt->field = V4L2_FIELD_NONE; | ||
496 | fmt->colorspace = V4L2_COLORSPACE_SRGB; | ||
497 | |||
498 | return 0; | ||
499 | } | ||
500 | |||
501 | static int mt9v011_g_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms) | ||
502 | { | ||
503 | struct v4l2_captureparm *cp = &parms->parm.capture; | ||
504 | |||
505 | if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) | ||
506 | return -EINVAL; | ||
507 | |||
508 | memset(cp, 0, sizeof(struct v4l2_captureparm)); | ||
509 | cp->capability = V4L2_CAP_TIMEPERFRAME; | ||
510 | calc_fps(sd, | ||
511 | &cp->timeperframe.numerator, | ||
512 | &cp->timeperframe.denominator); | ||
513 | |||
514 | return 0; | ||
515 | } | ||
516 | |||
517 | static int mt9v011_s_parm(struct v4l2_subdev *sd, struct v4l2_streamparm *parms) | ||
518 | { | ||
519 | struct v4l2_captureparm *cp = &parms->parm.capture; | ||
520 | struct v4l2_fract *tpf = &cp->timeperframe; | ||
521 | u16 speed; | ||
522 | |||
523 | if (parms->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) | ||
524 | return -EINVAL; | ||
525 | if (cp->extendedmode != 0) | ||
526 | return -EINVAL; | ||
527 | |||
528 | speed = calc_speed(sd, tpf->numerator, tpf->denominator); | ||
529 | |||
530 | mt9v011_write(sd, R0A_MT9V011_CLK_SPEED, speed); | ||
531 | v4l2_dbg(1, debug, sd, "Setting speed to %d\n", speed); | ||
532 | |||
533 | /* Recalculate and update fps info */ | ||
534 | calc_fps(sd, &tpf->numerator, &tpf->denominator); | ||
535 | |||
536 | return 0; | ||
537 | } | ||
538 | |||
539 | static int mt9v011_s_mbus_fmt(struct v4l2_subdev *sd, struct v4l2_mbus_framefmt *fmt) | ||
540 | { | ||
541 | struct mt9v011 *core = to_mt9v011(sd); | ||
542 | int rc; | ||
543 | |||
544 | rc = mt9v011_try_mbus_fmt(sd, fmt); | ||
545 | if (rc < 0) | ||
546 | return -EINVAL; | ||
547 | |||
548 | core->width = fmt->width; | ||
549 | core->height = fmt->height; | ||
550 | |||
551 | set_res(sd); | ||
552 | |||
553 | return 0; | ||
554 | } | ||
555 | |||
556 | #ifdef CONFIG_VIDEO_ADV_DEBUG | ||
557 | static int mt9v011_g_register(struct v4l2_subdev *sd, | ||
558 | struct v4l2_dbg_register *reg) | ||
559 | { | ||
560 | struct i2c_client *client = v4l2_get_subdevdata(sd); | ||
561 | |||
562 | if (!v4l2_chip_match_i2c_client(client, ®->match)) | ||
563 | return -EINVAL; | ||
564 | if (!capable(CAP_SYS_ADMIN)) | ||
565 | return -EPERM; | ||
566 | |||
567 | reg->val = mt9v011_read(sd, reg->reg & 0xff); | ||
568 | reg->size = 2; | ||
569 | |||
570 | return 0; | ||
571 | } | ||
572 | |||
573 | static int mt9v011_s_register(struct v4l2_subdev *sd, | ||
574 | struct v4l2_dbg_register *reg) | ||
575 | { | ||
576 | struct i2c_client *client = v4l2_get_subdevdata(sd); | ||
577 | |||
578 | if (!v4l2_chip_match_i2c_client(client, ®->match)) | ||
579 | return -EINVAL; | ||
580 | if (!capable(CAP_SYS_ADMIN)) | ||
581 | return -EPERM; | ||
582 | |||
583 | mt9v011_write(sd, reg->reg & 0xff, reg->val & 0xffff); | ||
584 | |||
585 | return 0; | ||
586 | } | ||
587 | #endif | ||
588 | |||
589 | static int mt9v011_g_chip_ident(struct v4l2_subdev *sd, | ||
590 | struct v4l2_dbg_chip_ident *chip) | ||
591 | { | ||
592 | u16 version; | ||
593 | struct i2c_client *client = v4l2_get_subdevdata(sd); | ||
594 | |||
595 | version = mt9v011_read(sd, R00_MT9V011_CHIP_VERSION); | ||
596 | |||
597 | return v4l2_chip_ident_i2c_client(client, chip, V4L2_IDENT_MT9V011, | ||
598 | version); | ||
599 | } | ||
600 | |||
601 | static const struct v4l2_subdev_core_ops mt9v011_core_ops = { | ||
602 | .queryctrl = mt9v011_queryctrl, | ||
603 | .g_ctrl = mt9v011_g_ctrl, | ||
604 | .s_ctrl = mt9v011_s_ctrl, | ||
605 | .reset = mt9v011_reset, | ||
606 | .g_chip_ident = mt9v011_g_chip_ident, | ||
607 | #ifdef CONFIG_VIDEO_ADV_DEBUG | ||
608 | .g_register = mt9v011_g_register, | ||
609 | .s_register = mt9v011_s_register, | ||
610 | #endif | ||
611 | }; | ||
612 | |||
613 | static const struct v4l2_subdev_video_ops mt9v011_video_ops = { | ||
614 | .enum_mbus_fmt = mt9v011_enum_mbus_fmt, | ||
615 | .try_mbus_fmt = mt9v011_try_mbus_fmt, | ||
616 | .s_mbus_fmt = mt9v011_s_mbus_fmt, | ||
617 | .g_parm = mt9v011_g_parm, | ||
618 | .s_parm = mt9v011_s_parm, | ||
619 | }; | ||
620 | |||
621 | static const struct v4l2_subdev_ops mt9v011_ops = { | ||
622 | .core = &mt9v011_core_ops, | ||
623 | .video = &mt9v011_video_ops, | ||
624 | }; | ||
625 | |||
626 | |||
627 | /**************************************************************************** | ||
628 | I2C Client & Driver | ||
629 | ****************************************************************************/ | ||
630 | |||
631 | static int mt9v011_probe(struct i2c_client *c, | ||
632 | const struct i2c_device_id *id) | ||
633 | { | ||
634 | u16 version; | ||
635 | struct mt9v011 *core; | ||
636 | struct v4l2_subdev *sd; | ||
637 | |||
638 | /* Check if the adapter supports the needed features */ | ||
639 | if (!i2c_check_functionality(c->adapter, | ||
640 | I2C_FUNC_SMBUS_READ_BYTE | I2C_FUNC_SMBUS_WRITE_BYTE_DATA)) | ||
641 | return -EIO; | ||
642 | |||
643 | core = kzalloc(sizeof(struct mt9v011), GFP_KERNEL); | ||
644 | if (!core) | ||
645 | return -ENOMEM; | ||
646 | |||
647 | sd = &core->sd; | ||
648 | v4l2_i2c_subdev_init(sd, c, &mt9v011_ops); | ||
649 | |||
650 | /* Check if the sensor is really a MT9V011 */ | ||
651 | version = mt9v011_read(sd, R00_MT9V011_CHIP_VERSION); | ||
652 | if ((version != MT9V011_VERSION) && | ||
653 | (version != MT9V011_REV_B_VERSION)) { | ||
654 | v4l2_info(sd, "*** unknown micron chip detected (0x%04x).\n", | ||
655 | version); | ||
656 | kfree(core); | ||
657 | return -EINVAL; | ||
658 | } | ||
659 | |||
660 | core->global_gain = 0x0024; | ||
661 | core->exposure = 0x01fc; | ||
662 | core->width = 640; | ||
663 | core->height = 480; | ||
664 | core->xtal = 27000000; /* Hz */ | ||
665 | |||
666 | if (c->dev.platform_data) { | ||
667 | struct mt9v011_platform_data *pdata = c->dev.platform_data; | ||
668 | |||
669 | core->xtal = pdata->xtal; | ||
670 | v4l2_dbg(1, debug, sd, "xtal set to %d.%03d MHz\n", | ||
671 | core->xtal / 1000000, (core->xtal / 1000) % 1000); | ||
672 | } | ||
673 | |||
674 | v4l_info(c, "chip found @ 0x%02x (%s - chip version 0x%04x)\n", | ||
675 | c->addr << 1, c->adapter->name, version); | ||
676 | |||
677 | return 0; | ||
678 | } | ||
679 | |||
680 | static int mt9v011_remove(struct i2c_client *c) | ||
681 | { | ||
682 | struct v4l2_subdev *sd = i2c_get_clientdata(c); | ||
683 | |||
684 | v4l2_dbg(1, debug, sd, | ||
685 | "mt9v011.c: removing mt9v011 adapter on address 0x%x\n", | ||
686 | c->addr << 1); | ||
687 | |||
688 | v4l2_device_unregister_subdev(sd); | ||
689 | kfree(to_mt9v011(sd)); | ||
690 | return 0; | ||
691 | } | ||
692 | |||
693 | /* ----------------------------------------------------------------------- */ | ||
694 | |||
695 | static const struct i2c_device_id mt9v011_id[] = { | ||
696 | { "mt9v011", 0 }, | ||
697 | { } | ||
698 | }; | ||
699 | MODULE_DEVICE_TABLE(i2c, mt9v011_id); | ||
700 | |||
701 | static struct i2c_driver mt9v011_driver = { | ||
702 | .driver = { | ||
703 | .owner = THIS_MODULE, | ||
704 | .name = "mt9v011", | ||
705 | }, | ||
706 | .probe = mt9v011_probe, | ||
707 | .remove = mt9v011_remove, | ||
708 | .id_table = mt9v011_id, | ||
709 | }; | ||
710 | |||
711 | static __init int init_mt9v011(void) | ||
712 | { | ||
713 | return i2c_add_driver(&mt9v011_driver); | ||
714 | } | ||
715 | |||
716 | static __exit void exit_mt9v011(void) | ||
717 | { | ||
718 | i2c_del_driver(&mt9v011_driver); | ||
719 | } | ||
720 | |||
721 | module_init(init_mt9v011); | ||
722 | module_exit(exit_mt9v011); | ||