1
2
3
4
5
6
7
8
9
10
11
12#include <linux/clk.h>
13#include <linux/delay.h>
14#include <linux/device.h>
15#include <linux/gpio/consumer.h>
16#include <linux/i2c.h>
17#include <linux/log2.h>
18#include <linux/module.h>
19#include <linux/of.h>
20#include <linux/of_graph.h>
21#include <linux/pm.h>
22#include <linux/regulator/consumer.h>
23#include <linux/slab.h>
24#include <linux/videodev2.h>
25
26#include <media/i2c/mt9p031.h>
27#include <media/v4l2-async.h>
28#include <media/v4l2-ctrls.h>
29#include <media/v4l2-device.h>
30#include <media/v4l2-subdev.h>
31
32#include "aptina-pll.h"
33
34#define MT9P031_PIXEL_ARRAY_WIDTH 2752
35#define MT9P031_PIXEL_ARRAY_HEIGHT 2004
36
37#define MT9P031_CHIP_VERSION 0x00
38#define MT9P031_CHIP_VERSION_VALUE 0x1801
39#define MT9P031_ROW_START 0x01
40#define MT9P031_ROW_START_MIN 0
41#define MT9P031_ROW_START_MAX 2004
42#define MT9P031_ROW_START_DEF 54
43#define MT9P031_COLUMN_START 0x02
44#define MT9P031_COLUMN_START_MIN 0
45#define MT9P031_COLUMN_START_MAX 2750
46#define MT9P031_COLUMN_START_DEF 16
47#define MT9P031_WINDOW_HEIGHT 0x03
48#define MT9P031_WINDOW_HEIGHT_MIN 2
49#define MT9P031_WINDOW_HEIGHT_MAX 2006
50#define MT9P031_WINDOW_HEIGHT_DEF 1944
51#define MT9P031_WINDOW_WIDTH 0x04
52#define MT9P031_WINDOW_WIDTH_MIN 2
53#define MT9P031_WINDOW_WIDTH_MAX 2752
54#define MT9P031_WINDOW_WIDTH_DEF 2592
55#define MT9P031_HORIZONTAL_BLANK 0x05
56#define MT9P031_HORIZONTAL_BLANK_MIN 0
57#define MT9P031_HORIZONTAL_BLANK_MAX 4095
58#define MT9P031_VERTICAL_BLANK 0x06
59#define MT9P031_VERTICAL_BLANK_MIN 1
60#define MT9P031_VERTICAL_BLANK_MAX 4096
61#define MT9P031_VERTICAL_BLANK_DEF 26
62#define MT9P031_OUTPUT_CONTROL 0x07
63#define MT9P031_OUTPUT_CONTROL_CEN 2
64#define MT9P031_OUTPUT_CONTROL_SYN 1
65#define MT9P031_OUTPUT_CONTROL_DEF 0x1f82
66#define MT9P031_SHUTTER_WIDTH_UPPER 0x08
67#define MT9P031_SHUTTER_WIDTH_LOWER 0x09
68#define MT9P031_SHUTTER_WIDTH_MIN 1
69#define MT9P031_SHUTTER_WIDTH_MAX 1048575
70#define MT9P031_SHUTTER_WIDTH_DEF 1943
71#define MT9P031_PLL_CONTROL 0x10
72#define MT9P031_PLL_CONTROL_PWROFF 0x0050
73#define MT9P031_PLL_CONTROL_PWRON 0x0051
74#define MT9P031_PLL_CONTROL_USEPLL 0x0052
75#define MT9P031_PLL_CONFIG_1 0x11
76#define MT9P031_PLL_CONFIG_2 0x12
77#define MT9P031_PIXEL_CLOCK_CONTROL 0x0a
78#define MT9P031_PIXEL_CLOCK_INVERT (1 << 15)
79#define MT9P031_PIXEL_CLOCK_SHIFT(n) ((n) << 8)
80#define MT9P031_PIXEL_CLOCK_DIVIDE(n) ((n) << 0)
81#define MT9P031_FRAME_RESTART 0x0b
82#define MT9P031_SHUTTER_DELAY 0x0c
83#define MT9P031_RST 0x0d
84#define MT9P031_RST_ENABLE 1
85#define MT9P031_RST_DISABLE 0
86#define MT9P031_READ_MODE_1 0x1e
87#define MT9P031_READ_MODE_2 0x20
88#define MT9P031_READ_MODE_2_ROW_MIR (1 << 15)
89#define MT9P031_READ_MODE_2_COL_MIR (1 << 14)
90#define MT9P031_READ_MODE_2_ROW_BLC (1 << 6)
91#define MT9P031_ROW_ADDRESS_MODE 0x22
92#define MT9P031_COLUMN_ADDRESS_MODE 0x23
93#define MT9P031_GLOBAL_GAIN 0x35
94#define MT9P031_GLOBAL_GAIN_MIN 8
95#define MT9P031_GLOBAL_GAIN_MAX 1024
96#define MT9P031_GLOBAL_GAIN_DEF 8
97#define MT9P031_GLOBAL_GAIN_MULT (1 << 6)
98#define MT9P031_ROW_BLACK_TARGET 0x49
99#define MT9P031_ROW_BLACK_DEF_OFFSET 0x4b
100#define MT9P031_GREEN1_OFFSET 0x60
101#define MT9P031_GREEN2_OFFSET 0x61
102#define MT9P031_BLACK_LEVEL_CALIBRATION 0x62
103#define MT9P031_BLC_MANUAL_BLC (1 << 0)
104#define MT9P031_RED_OFFSET 0x63
105#define MT9P031_BLUE_OFFSET 0x64
106#define MT9P031_TEST_PATTERN 0xa0
107#define MT9P031_TEST_PATTERN_SHIFT 3
108#define MT9P031_TEST_PATTERN_ENABLE (1 << 0)
109#define MT9P031_TEST_PATTERN_DISABLE (0 << 0)
110#define MT9P031_TEST_PATTERN_GREEN 0xa1
111#define MT9P031_TEST_PATTERN_RED 0xa2
112#define MT9P031_TEST_PATTERN_BLUE 0xa3
113
114enum mt9p031_model {
115 MT9P031_MODEL_COLOR,
116 MT9P031_MODEL_MONOCHROME,
117};
118
119struct mt9p031 {
120 struct v4l2_subdev subdev;
121 struct media_pad pad;
122 struct v4l2_rect crop;
123 struct v4l2_mbus_framefmt format;
124 struct mt9p031_platform_data *pdata;
125 struct mutex power_lock;
126 int power_count;
127
128 struct clk *clk;
129 struct regulator_bulk_data regulators[3];
130
131 enum mt9p031_model model;
132 struct aptina_pll pll;
133 unsigned int clk_div;
134 bool use_pll;
135 struct gpio_desc *reset;
136
137 struct v4l2_ctrl_handler ctrls;
138 struct v4l2_ctrl *blc_auto;
139 struct v4l2_ctrl *blc_offset;
140
141
142 u16 output_control;
143 u16 mode2;
144};
145
146static struct mt9p031 *to_mt9p031(struct v4l2_subdev *sd)
147{
148 return container_of(sd, struct mt9p031, subdev);
149}
150
151static int mt9p031_read(struct i2c_client *client, u8 reg)
152{
153 return i2c_smbus_read_word_swapped(client, reg);
154}
155
156static int mt9p031_write(struct i2c_client *client, u8 reg, u16 data)
157{
158 return i2c_smbus_write_word_swapped(client, reg, data);
159}
160
161static int mt9p031_set_output_control(struct mt9p031 *mt9p031, u16 clear,
162 u16 set)
163{
164 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
165 u16 value = (mt9p031->output_control & ~clear) | set;
166 int ret;
167
168 ret = mt9p031_write(client, MT9P031_OUTPUT_CONTROL, value);
169 if (ret < 0)
170 return ret;
171
172 mt9p031->output_control = value;
173 return 0;
174}
175
176static int mt9p031_set_mode2(struct mt9p031 *mt9p031, u16 clear, u16 set)
177{
178 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
179 u16 value = (mt9p031->mode2 & ~clear) | set;
180 int ret;
181
182 ret = mt9p031_write(client, MT9P031_READ_MODE_2, value);
183 if (ret < 0)
184 return ret;
185
186 mt9p031->mode2 = value;
187 return 0;
188}
189
190static int mt9p031_reset(struct mt9p031 *mt9p031)
191{
192 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
193 int ret;
194
195
196 ret = mt9p031_write(client, MT9P031_RST, MT9P031_RST_ENABLE);
197 if (ret < 0)
198 return ret;
199 ret = mt9p031_write(client, MT9P031_RST, MT9P031_RST_DISABLE);
200 if (ret < 0)
201 return ret;
202
203 ret = mt9p031_write(client, MT9P031_PIXEL_CLOCK_CONTROL,
204 MT9P031_PIXEL_CLOCK_DIVIDE(mt9p031->clk_div));
205 if (ret < 0)
206 return ret;
207
208 return mt9p031_set_output_control(mt9p031, MT9P031_OUTPUT_CONTROL_CEN,
209 0);
210}
211
212static int mt9p031_clk_setup(struct mt9p031 *mt9p031)
213{
214 static const struct aptina_pll_limits limits = {
215 .ext_clock_min = 6000000,
216 .ext_clock_max = 27000000,
217 .int_clock_min = 2000000,
218 .int_clock_max = 13500000,
219 .out_clock_min = 180000000,
220 .out_clock_max = 360000000,
221 .pix_clock_max = 96000000,
222 .n_min = 1,
223 .n_max = 64,
224 .m_min = 16,
225 .m_max = 255,
226 .p1_min = 1,
227 .p1_max = 128,
228 };
229
230 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
231 struct mt9p031_platform_data *pdata = mt9p031->pdata;
232 int ret;
233
234 mt9p031->clk = devm_clk_get(&client->dev, NULL);
235 if (IS_ERR(mt9p031->clk))
236 return PTR_ERR(mt9p031->clk);
237
238 ret = clk_set_rate(mt9p031->clk, pdata->ext_freq);
239 if (ret < 0)
240 return ret;
241
242
243
244
245 if (pdata->ext_freq > limits.ext_clock_max) {
246 unsigned int div;
247
248 div = DIV_ROUND_UP(pdata->ext_freq, pdata->target_freq);
249 div = roundup_pow_of_two(div) / 2;
250
251 mt9p031->clk_div = min_t(unsigned int, div, 64);
252 mt9p031->use_pll = false;
253
254 return 0;
255 }
256
257 mt9p031->pll.ext_clock = pdata->ext_freq;
258 mt9p031->pll.pix_clock = pdata->target_freq;
259 mt9p031->use_pll = true;
260
261 return aptina_pll_calculate(&client->dev, &limits, &mt9p031->pll);
262}
263
264static int mt9p031_pll_enable(struct mt9p031 *mt9p031)
265{
266 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
267 int ret;
268
269 if (!mt9p031->use_pll)
270 return 0;
271
272 ret = mt9p031_write(client, MT9P031_PLL_CONTROL,
273 MT9P031_PLL_CONTROL_PWRON);
274 if (ret < 0)
275 return ret;
276
277 ret = mt9p031_write(client, MT9P031_PLL_CONFIG_1,
278 (mt9p031->pll.m << 8) | (mt9p031->pll.n - 1));
279 if (ret < 0)
280 return ret;
281
282 ret = mt9p031_write(client, MT9P031_PLL_CONFIG_2, mt9p031->pll.p1 - 1);
283 if (ret < 0)
284 return ret;
285
286 usleep_range(1000, 2000);
287 ret = mt9p031_write(client, MT9P031_PLL_CONTROL,
288 MT9P031_PLL_CONTROL_PWRON |
289 MT9P031_PLL_CONTROL_USEPLL);
290 return ret;
291}
292
293static inline int mt9p031_pll_disable(struct mt9p031 *mt9p031)
294{
295 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
296
297 if (!mt9p031->use_pll)
298 return 0;
299
300 return mt9p031_write(client, MT9P031_PLL_CONTROL,
301 MT9P031_PLL_CONTROL_PWROFF);
302}
303
304static int mt9p031_power_on(struct mt9p031 *mt9p031)
305{
306 int ret;
307
308
309 if (mt9p031->reset) {
310 gpiod_set_value(mt9p031->reset, 1);
311 usleep_range(1000, 2000);
312 }
313
314
315 ret = regulator_bulk_enable(ARRAY_SIZE(mt9p031->regulators),
316 mt9p031->regulators);
317 if (ret < 0)
318 return ret;
319
320
321 if (mt9p031->clk) {
322 ret = clk_prepare_enable(mt9p031->clk);
323 if (ret) {
324 regulator_bulk_disable(ARRAY_SIZE(mt9p031->regulators),
325 mt9p031->regulators);
326 return ret;
327 }
328 }
329
330
331 if (mt9p031->reset) {
332 gpiod_set_value(mt9p031->reset, 0);
333 usleep_range(1000, 2000);
334 }
335
336 return 0;
337}
338
339static void mt9p031_power_off(struct mt9p031 *mt9p031)
340{
341 if (mt9p031->reset) {
342 gpiod_set_value(mt9p031->reset, 1);
343 usleep_range(1000, 2000);
344 }
345
346 regulator_bulk_disable(ARRAY_SIZE(mt9p031->regulators),
347 mt9p031->regulators);
348
349 clk_disable_unprepare(mt9p031->clk);
350}
351
352static int __mt9p031_set_power(struct mt9p031 *mt9p031, bool on)
353{
354 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
355 int ret;
356
357 if (!on) {
358 mt9p031_power_off(mt9p031);
359 return 0;
360 }
361
362 ret = mt9p031_power_on(mt9p031);
363 if (ret < 0)
364 return ret;
365
366 ret = mt9p031_reset(mt9p031);
367 if (ret < 0) {
368 dev_err(&client->dev, "Failed to reset the camera\n");
369 return ret;
370 }
371
372 return v4l2_ctrl_handler_setup(&mt9p031->ctrls);
373}
374
375
376
377
378
379static int mt9p031_set_params(struct mt9p031 *mt9p031)
380{
381 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
382 struct v4l2_mbus_framefmt *format = &mt9p031->format;
383 const struct v4l2_rect *crop = &mt9p031->crop;
384 unsigned int hblank;
385 unsigned int vblank;
386 unsigned int xskip;
387 unsigned int yskip;
388 unsigned int xbin;
389 unsigned int ybin;
390 int ret;
391
392
393
394
395
396
397
398 ret = mt9p031_write(client, MT9P031_COLUMN_START, crop->left);
399 if (ret < 0)
400 return ret;
401 ret = mt9p031_write(client, MT9P031_ROW_START, crop->top);
402 if (ret < 0)
403 return ret;
404 ret = mt9p031_write(client, MT9P031_WINDOW_WIDTH, crop->width - 1);
405 if (ret < 0)
406 return ret;
407 ret = mt9p031_write(client, MT9P031_WINDOW_HEIGHT, crop->height - 1);
408 if (ret < 0)
409 return ret;
410
411
412
413
414 xskip = DIV_ROUND_CLOSEST(crop->width, format->width);
415 yskip = DIV_ROUND_CLOSEST(crop->height, format->height);
416 xbin = 1 << (ffs(xskip) - 1);
417 ybin = 1 << (ffs(yskip) - 1);
418
419 ret = mt9p031_write(client, MT9P031_COLUMN_ADDRESS_MODE,
420 ((xbin - 1) << 4) | (xskip - 1));
421 if (ret < 0)
422 return ret;
423 ret = mt9p031_write(client, MT9P031_ROW_ADDRESS_MODE,
424 ((ybin - 1) << 4) | (yskip - 1));
425 if (ret < 0)
426 return ret;
427
428
429
430
431 hblank = 346 * ybin + 64 + (80 >> min_t(unsigned int, xbin, 3));
432 vblank = MT9P031_VERTICAL_BLANK_DEF;
433
434 ret = mt9p031_write(client, MT9P031_HORIZONTAL_BLANK, hblank - 1);
435 if (ret < 0)
436 return ret;
437 ret = mt9p031_write(client, MT9P031_VERTICAL_BLANK, vblank - 1);
438 if (ret < 0)
439 return ret;
440
441 return ret;
442}
443
444static int mt9p031_s_stream(struct v4l2_subdev *subdev, int enable)
445{
446 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
447 int ret;
448
449 if (!enable) {
450
451 ret = mt9p031_set_output_control(mt9p031,
452 MT9P031_OUTPUT_CONTROL_CEN, 0);
453 if (ret < 0)
454 return ret;
455
456 return mt9p031_pll_disable(mt9p031);
457 }
458
459 ret = mt9p031_set_params(mt9p031);
460 if (ret < 0)
461 return ret;
462
463
464 ret = mt9p031_set_output_control(mt9p031, 0,
465 MT9P031_OUTPUT_CONTROL_CEN);
466 if (ret < 0)
467 return ret;
468
469 return mt9p031_pll_enable(mt9p031);
470}
471
472static int mt9p031_enum_mbus_code(struct v4l2_subdev *subdev,
473 struct v4l2_subdev_pad_config *cfg,
474 struct v4l2_subdev_mbus_code_enum *code)
475{
476 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
477
478 if (code->pad || code->index)
479 return -EINVAL;
480
481 code->code = mt9p031->format.code;
482 return 0;
483}
484
485static int mt9p031_enum_frame_size(struct v4l2_subdev *subdev,
486 struct v4l2_subdev_pad_config *cfg,
487 struct v4l2_subdev_frame_size_enum *fse)
488{
489 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
490
491 if (fse->index >= 8 || fse->code != mt9p031->format.code)
492 return -EINVAL;
493
494 fse->min_width = MT9P031_WINDOW_WIDTH_DEF
495 / min_t(unsigned int, 7, fse->index + 1);
496 fse->max_width = fse->min_width;
497 fse->min_height = MT9P031_WINDOW_HEIGHT_DEF / (fse->index + 1);
498 fse->max_height = fse->min_height;
499
500 return 0;
501}
502
503static struct v4l2_mbus_framefmt *
504__mt9p031_get_pad_format(struct mt9p031 *mt9p031, struct v4l2_subdev_pad_config *cfg,
505 unsigned int pad, u32 which)
506{
507 switch (which) {
508 case V4L2_SUBDEV_FORMAT_TRY:
509 return v4l2_subdev_get_try_format(&mt9p031->subdev, cfg, pad);
510 case V4L2_SUBDEV_FORMAT_ACTIVE:
511 return &mt9p031->format;
512 default:
513 return NULL;
514 }
515}
516
517static struct v4l2_rect *
518__mt9p031_get_pad_crop(struct mt9p031 *mt9p031, struct v4l2_subdev_pad_config *cfg,
519 unsigned int pad, u32 which)
520{
521 switch (which) {
522 case V4L2_SUBDEV_FORMAT_TRY:
523 return v4l2_subdev_get_try_crop(&mt9p031->subdev, cfg, pad);
524 case V4L2_SUBDEV_FORMAT_ACTIVE:
525 return &mt9p031->crop;
526 default:
527 return NULL;
528 }
529}
530
531static int mt9p031_get_format(struct v4l2_subdev *subdev,
532 struct v4l2_subdev_pad_config *cfg,
533 struct v4l2_subdev_format *fmt)
534{
535 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
536
537 fmt->format = *__mt9p031_get_pad_format(mt9p031, cfg, fmt->pad,
538 fmt->which);
539 return 0;
540}
541
542static int mt9p031_set_format(struct v4l2_subdev *subdev,
543 struct v4l2_subdev_pad_config *cfg,
544 struct v4l2_subdev_format *format)
545{
546 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
547 struct v4l2_mbus_framefmt *__format;
548 struct v4l2_rect *__crop;
549 unsigned int width;
550 unsigned int height;
551 unsigned int hratio;
552 unsigned int vratio;
553
554 __crop = __mt9p031_get_pad_crop(mt9p031, cfg, format->pad,
555 format->which);
556
557
558 width = clamp_t(unsigned int, ALIGN(format->format.width, 2),
559 max_t(unsigned int, __crop->width / 7,
560 MT9P031_WINDOW_WIDTH_MIN),
561 __crop->width);
562 height = clamp_t(unsigned int, ALIGN(format->format.height, 2),
563 max_t(unsigned int, __crop->height / 8,
564 MT9P031_WINDOW_HEIGHT_MIN),
565 __crop->height);
566
567 hratio = DIV_ROUND_CLOSEST(__crop->width, width);
568 vratio = DIV_ROUND_CLOSEST(__crop->height, height);
569
570 __format = __mt9p031_get_pad_format(mt9p031, cfg, format->pad,
571 format->which);
572 __format->width = __crop->width / hratio;
573 __format->height = __crop->height / vratio;
574
575 format->format = *__format;
576
577 return 0;
578}
579
580static int mt9p031_get_selection(struct v4l2_subdev *subdev,
581 struct v4l2_subdev_pad_config *cfg,
582 struct v4l2_subdev_selection *sel)
583{
584 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
585
586 if (sel->target != V4L2_SEL_TGT_CROP)
587 return -EINVAL;
588
589 sel->r = *__mt9p031_get_pad_crop(mt9p031, cfg, sel->pad, sel->which);
590 return 0;
591}
592
593static int mt9p031_set_selection(struct v4l2_subdev *subdev,
594 struct v4l2_subdev_pad_config *cfg,
595 struct v4l2_subdev_selection *sel)
596{
597 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
598 struct v4l2_mbus_framefmt *__format;
599 struct v4l2_rect *__crop;
600 struct v4l2_rect rect;
601
602 if (sel->target != V4L2_SEL_TGT_CROP)
603 return -EINVAL;
604
605
606
607
608 rect.left = clamp(ALIGN(sel->r.left, 2), MT9P031_COLUMN_START_MIN,
609 MT9P031_COLUMN_START_MAX);
610 rect.top = clamp(ALIGN(sel->r.top, 2), MT9P031_ROW_START_MIN,
611 MT9P031_ROW_START_MAX);
612 rect.width = clamp_t(unsigned int, ALIGN(sel->r.width, 2),
613 MT9P031_WINDOW_WIDTH_MIN,
614 MT9P031_WINDOW_WIDTH_MAX);
615 rect.height = clamp_t(unsigned int, ALIGN(sel->r.height, 2),
616 MT9P031_WINDOW_HEIGHT_MIN,
617 MT9P031_WINDOW_HEIGHT_MAX);
618
619 rect.width = min_t(unsigned int, rect.width,
620 MT9P031_PIXEL_ARRAY_WIDTH - rect.left);
621 rect.height = min_t(unsigned int, rect.height,
622 MT9P031_PIXEL_ARRAY_HEIGHT - rect.top);
623
624 __crop = __mt9p031_get_pad_crop(mt9p031, cfg, sel->pad, sel->which);
625
626 if (rect.width != __crop->width || rect.height != __crop->height) {
627
628
629
630 __format = __mt9p031_get_pad_format(mt9p031, cfg, sel->pad,
631 sel->which);
632 __format->width = rect.width;
633 __format->height = rect.height;
634 }
635
636 *__crop = rect;
637 sel->r = rect;
638
639 return 0;
640}
641
642
643
644
645
646#define V4L2_CID_BLC_AUTO (V4L2_CID_USER_BASE | 0x1002)
647#define V4L2_CID_BLC_TARGET_LEVEL (V4L2_CID_USER_BASE | 0x1003)
648#define V4L2_CID_BLC_ANALOG_OFFSET (V4L2_CID_USER_BASE | 0x1004)
649#define V4L2_CID_BLC_DIGITAL_OFFSET (V4L2_CID_USER_BASE | 0x1005)
650
651static int mt9p031_restore_blc(struct mt9p031 *mt9p031)
652{
653 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
654 int ret;
655
656 if (mt9p031->blc_auto->cur.val != 0) {
657 ret = mt9p031_set_mode2(mt9p031, 0,
658 MT9P031_READ_MODE_2_ROW_BLC);
659 if (ret < 0)
660 return ret;
661 }
662
663 if (mt9p031->blc_offset->cur.val != 0) {
664 ret = mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
665 mt9p031->blc_offset->cur.val);
666 if (ret < 0)
667 return ret;
668 }
669
670 return 0;
671}
672
673static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
674{
675 struct mt9p031 *mt9p031 =
676 container_of(ctrl->handler, struct mt9p031, ctrls);
677 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
678 u16 data;
679 int ret;
680
681 if (ctrl->flags & V4L2_CTRL_FLAG_INACTIVE)
682 return 0;
683
684 switch (ctrl->id) {
685 case V4L2_CID_EXPOSURE:
686 ret = mt9p031_write(client, MT9P031_SHUTTER_WIDTH_UPPER,
687 (ctrl->val >> 16) & 0xffff);
688 if (ret < 0)
689 return ret;
690
691 return mt9p031_write(client, MT9P031_SHUTTER_WIDTH_LOWER,
692 ctrl->val & 0xffff);
693
694 case V4L2_CID_GAIN:
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709 if (ctrl->val <= 32) {
710 data = ctrl->val;
711 } else if (ctrl->val <= 64) {
712 ctrl->val &= ~1;
713 data = (1 << 6) | (ctrl->val >> 1);
714 } else {
715 ctrl->val &= ~7;
716 data = ((ctrl->val - 64) << 5) | (1 << 6) | 32;
717 }
718
719 return mt9p031_write(client, MT9P031_GLOBAL_GAIN, data);
720
721 case V4L2_CID_HFLIP:
722 if (ctrl->val)
723 return mt9p031_set_mode2(mt9p031,
724 0, MT9P031_READ_MODE_2_COL_MIR);
725 else
726 return mt9p031_set_mode2(mt9p031,
727 MT9P031_READ_MODE_2_COL_MIR, 0);
728
729 case V4L2_CID_VFLIP:
730 if (ctrl->val)
731 return mt9p031_set_mode2(mt9p031,
732 0, MT9P031_READ_MODE_2_ROW_MIR);
733 else
734 return mt9p031_set_mode2(mt9p031,
735 MT9P031_READ_MODE_2_ROW_MIR, 0);
736
737 case V4L2_CID_TEST_PATTERN:
738
739
740
741
742
743 v4l2_ctrl_activate(mt9p031->blc_auto, ctrl->val == 0);
744 v4l2_ctrl_activate(mt9p031->blc_offset, ctrl->val == 0);
745
746 if (!ctrl->val) {
747
748 ret = mt9p031_restore_blc(mt9p031);
749 if (ret < 0)
750 return ret;
751
752 return mt9p031_write(client, MT9P031_TEST_PATTERN,
753 MT9P031_TEST_PATTERN_DISABLE);
754 }
755
756 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_GREEN, 0x05a0);
757 if (ret < 0)
758 return ret;
759 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_RED, 0x0a50);
760 if (ret < 0)
761 return ret;
762 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_BLUE, 0x0aa0);
763 if (ret < 0)
764 return ret;
765
766
767 ret = mt9p031_set_mode2(mt9p031, MT9P031_READ_MODE_2_ROW_BLC,
768 0);
769 if (ret < 0)
770 return ret;
771
772 ret = mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET, 0);
773 if (ret < 0)
774 return ret;
775
776 return mt9p031_write(client, MT9P031_TEST_PATTERN,
777 ((ctrl->val - 1) << MT9P031_TEST_PATTERN_SHIFT)
778 | MT9P031_TEST_PATTERN_ENABLE);
779
780 case V4L2_CID_BLC_AUTO:
781 ret = mt9p031_set_mode2(mt9p031,
782 ctrl->val ? 0 : MT9P031_READ_MODE_2_ROW_BLC,
783 ctrl->val ? MT9P031_READ_MODE_2_ROW_BLC : 0);
784 if (ret < 0)
785 return ret;
786
787 return mt9p031_write(client, MT9P031_BLACK_LEVEL_CALIBRATION,
788 ctrl->val ? 0 : MT9P031_BLC_MANUAL_BLC);
789
790 case V4L2_CID_BLC_TARGET_LEVEL:
791 return mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
792 ctrl->val);
793
794 case V4L2_CID_BLC_ANALOG_OFFSET:
795 data = ctrl->val & ((1 << 9) - 1);
796
797 ret = mt9p031_write(client, MT9P031_GREEN1_OFFSET, data);
798 if (ret < 0)
799 return ret;
800 ret = mt9p031_write(client, MT9P031_GREEN2_OFFSET, data);
801 if (ret < 0)
802 return ret;
803 ret = mt9p031_write(client, MT9P031_RED_OFFSET, data);
804 if (ret < 0)
805 return ret;
806 return mt9p031_write(client, MT9P031_BLUE_OFFSET, data);
807
808 case V4L2_CID_BLC_DIGITAL_OFFSET:
809 return mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET,
810 ctrl->val & ((1 << 12) - 1));
811 }
812
813 return 0;
814}
815
816static const struct v4l2_ctrl_ops mt9p031_ctrl_ops = {
817 .s_ctrl = mt9p031_s_ctrl,
818};
819
820static const char * const mt9p031_test_pattern_menu[] = {
821 "Disabled",
822 "Color Field",
823 "Horizontal Gradient",
824 "Vertical Gradient",
825 "Diagonal Gradient",
826 "Classic Test Pattern",
827 "Walking 1s",
828 "Monochrome Horizontal Bars",
829 "Monochrome Vertical Bars",
830 "Vertical Color Bars",
831};
832
833static const struct v4l2_ctrl_config mt9p031_ctrls[] = {
834 {
835 .ops = &mt9p031_ctrl_ops,
836 .id = V4L2_CID_BLC_AUTO,
837 .type = V4L2_CTRL_TYPE_BOOLEAN,
838 .name = "BLC, Auto",
839 .min = 0,
840 .max = 1,
841 .step = 1,
842 .def = 1,
843 .flags = 0,
844 }, {
845 .ops = &mt9p031_ctrl_ops,
846 .id = V4L2_CID_BLC_TARGET_LEVEL,
847 .type = V4L2_CTRL_TYPE_INTEGER,
848 .name = "BLC Target Level",
849 .min = 0,
850 .max = 4095,
851 .step = 1,
852 .def = 168,
853 .flags = 0,
854 }, {
855 .ops = &mt9p031_ctrl_ops,
856 .id = V4L2_CID_BLC_ANALOG_OFFSET,
857 .type = V4L2_CTRL_TYPE_INTEGER,
858 .name = "BLC Analog Offset",
859 .min = -255,
860 .max = 255,
861 .step = 1,
862 .def = 32,
863 .flags = 0,
864 }, {
865 .ops = &mt9p031_ctrl_ops,
866 .id = V4L2_CID_BLC_DIGITAL_OFFSET,
867 .type = V4L2_CTRL_TYPE_INTEGER,
868 .name = "BLC Digital Offset",
869 .min = -2048,
870 .max = 2047,
871 .step = 1,
872 .def = 40,
873 .flags = 0,
874 }
875};
876
877
878
879
880
881static int mt9p031_set_power(struct v4l2_subdev *subdev, int on)
882{
883 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
884 int ret = 0;
885
886 mutex_lock(&mt9p031->power_lock);
887
888
889
890
891 if (mt9p031->power_count == !on) {
892 ret = __mt9p031_set_power(mt9p031, !!on);
893 if (ret < 0)
894 goto out;
895 }
896
897
898 mt9p031->power_count += on ? 1 : -1;
899 WARN_ON(mt9p031->power_count < 0);
900
901out:
902 mutex_unlock(&mt9p031->power_lock);
903 return ret;
904}
905
906
907
908
909
910static int mt9p031_registered(struct v4l2_subdev *subdev)
911{
912 struct i2c_client *client = v4l2_get_subdevdata(subdev);
913 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
914 s32 data;
915 int ret;
916
917 ret = mt9p031_power_on(mt9p031);
918 if (ret < 0) {
919 dev_err(&client->dev, "MT9P031 power up failed\n");
920 return ret;
921 }
922
923
924 data = mt9p031_read(client, MT9P031_CHIP_VERSION);
925 mt9p031_power_off(mt9p031);
926
927 if (data != MT9P031_CHIP_VERSION_VALUE) {
928 dev_err(&client->dev, "MT9P031 not detected, wrong version "
929 "0x%04x\n", data);
930 return -ENODEV;
931 }
932
933 dev_info(&client->dev, "MT9P031 detected at address 0x%02x\n",
934 client->addr);
935
936 return 0;
937}
938
939static int mt9p031_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
940{
941 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
942 struct v4l2_mbus_framefmt *format;
943 struct v4l2_rect *crop;
944
945 crop = v4l2_subdev_get_try_crop(subdev, fh->pad, 0);
946 crop->left = MT9P031_COLUMN_START_DEF;
947 crop->top = MT9P031_ROW_START_DEF;
948 crop->width = MT9P031_WINDOW_WIDTH_DEF;
949 crop->height = MT9P031_WINDOW_HEIGHT_DEF;
950
951 format = v4l2_subdev_get_try_format(subdev, fh->pad, 0);
952
953 if (mt9p031->model == MT9P031_MODEL_MONOCHROME)
954 format->code = MEDIA_BUS_FMT_Y12_1X12;
955 else
956 format->code = MEDIA_BUS_FMT_SGRBG12_1X12;
957
958 format->width = MT9P031_WINDOW_WIDTH_DEF;
959 format->height = MT9P031_WINDOW_HEIGHT_DEF;
960 format->field = V4L2_FIELD_NONE;
961 format->colorspace = V4L2_COLORSPACE_SRGB;
962
963 return mt9p031_set_power(subdev, 1);
964}
965
966static int mt9p031_close(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
967{
968 return mt9p031_set_power(subdev, 0);
969}
970
971static const struct v4l2_subdev_core_ops mt9p031_subdev_core_ops = {
972 .s_power = mt9p031_set_power,
973};
974
975static const struct v4l2_subdev_video_ops mt9p031_subdev_video_ops = {
976 .s_stream = mt9p031_s_stream,
977};
978
979static const struct v4l2_subdev_pad_ops mt9p031_subdev_pad_ops = {
980 .enum_mbus_code = mt9p031_enum_mbus_code,
981 .enum_frame_size = mt9p031_enum_frame_size,
982 .get_fmt = mt9p031_get_format,
983 .set_fmt = mt9p031_set_format,
984 .get_selection = mt9p031_get_selection,
985 .set_selection = mt9p031_set_selection,
986};
987
988static const struct v4l2_subdev_ops mt9p031_subdev_ops = {
989 .core = &mt9p031_subdev_core_ops,
990 .video = &mt9p031_subdev_video_ops,
991 .pad = &mt9p031_subdev_pad_ops,
992};
993
994static const struct v4l2_subdev_internal_ops mt9p031_subdev_internal_ops = {
995 .registered = mt9p031_registered,
996 .open = mt9p031_open,
997 .close = mt9p031_close,
998};
999
1000
1001
1002
1003
1004static struct mt9p031_platform_data *
1005mt9p031_get_pdata(struct i2c_client *client)
1006{
1007 struct mt9p031_platform_data *pdata;
1008 struct device_node *np;
1009
1010 if (!IS_ENABLED(CONFIG_OF) || !client->dev.of_node)
1011 return client->dev.platform_data;
1012
1013 np = of_graph_get_next_endpoint(client->dev.of_node, NULL);
1014 if (!np)
1015 return NULL;
1016
1017 pdata = devm_kzalloc(&client->dev, sizeof(*pdata), GFP_KERNEL);
1018 if (!pdata)
1019 goto done;
1020
1021 of_property_read_u32(np, "input-clock-frequency", &pdata->ext_freq);
1022 of_property_read_u32(np, "pixel-clock-frequency", &pdata->target_freq);
1023
1024done:
1025 of_node_put(np);
1026 return pdata;
1027}
1028
1029static int mt9p031_probe(struct i2c_client *client,
1030 const struct i2c_device_id *did)
1031{
1032 struct mt9p031_platform_data *pdata = mt9p031_get_pdata(client);
1033 struct i2c_adapter *adapter = client->adapter;
1034 struct mt9p031 *mt9p031;
1035 unsigned int i;
1036 int ret;
1037
1038 if (pdata == NULL) {
1039 dev_err(&client->dev, "No platform data\n");
1040 return -EINVAL;
1041 }
1042
1043 if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
1044 dev_warn(&client->dev,
1045 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
1046 return -EIO;
1047 }
1048
1049 mt9p031 = devm_kzalloc(&client->dev, sizeof(*mt9p031), GFP_KERNEL);
1050 if (mt9p031 == NULL)
1051 return -ENOMEM;
1052
1053 mt9p031->pdata = pdata;
1054 mt9p031->output_control = MT9P031_OUTPUT_CONTROL_DEF;
1055 mt9p031->mode2 = MT9P031_READ_MODE_2_ROW_BLC;
1056 mt9p031->model = did->driver_data;
1057
1058 mt9p031->regulators[0].supply = "vdd";
1059 mt9p031->regulators[1].supply = "vdd_io";
1060 mt9p031->regulators[2].supply = "vaa";
1061
1062 ret = devm_regulator_bulk_get(&client->dev, 3, mt9p031->regulators);
1063 if (ret < 0) {
1064 dev_err(&client->dev, "Unable to get regulators\n");
1065 return ret;
1066 }
1067
1068 mutex_init(&mt9p031->power_lock);
1069
1070 v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 6);
1071
1072 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1073 V4L2_CID_EXPOSURE, MT9P031_SHUTTER_WIDTH_MIN,
1074 MT9P031_SHUTTER_WIDTH_MAX, 1,
1075 MT9P031_SHUTTER_WIDTH_DEF);
1076 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1077 V4L2_CID_GAIN, MT9P031_GLOBAL_GAIN_MIN,
1078 MT9P031_GLOBAL_GAIN_MAX, 1, MT9P031_GLOBAL_GAIN_DEF);
1079 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1080 V4L2_CID_HFLIP, 0, 1, 1, 0);
1081 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1082 V4L2_CID_VFLIP, 0, 1, 1, 0);
1083 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1084 V4L2_CID_PIXEL_RATE, pdata->target_freq,
1085 pdata->target_freq, 1, pdata->target_freq);
1086 v4l2_ctrl_new_std_menu_items(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1087 V4L2_CID_TEST_PATTERN,
1088 ARRAY_SIZE(mt9p031_test_pattern_menu) - 1, 0,
1089 0, mt9p031_test_pattern_menu);
1090
1091 for (i = 0; i < ARRAY_SIZE(mt9p031_ctrls); ++i)
1092 v4l2_ctrl_new_custom(&mt9p031->ctrls, &mt9p031_ctrls[i], NULL);
1093
1094 mt9p031->subdev.ctrl_handler = &mt9p031->ctrls;
1095
1096 if (mt9p031->ctrls.error) {
1097 printk(KERN_INFO "%s: control initialization error %d\n",
1098 __func__, mt9p031->ctrls.error);
1099 ret = mt9p031->ctrls.error;
1100 goto done;
1101 }
1102
1103 mt9p031->blc_auto = v4l2_ctrl_find(&mt9p031->ctrls, V4L2_CID_BLC_AUTO);
1104 mt9p031->blc_offset = v4l2_ctrl_find(&mt9p031->ctrls,
1105 V4L2_CID_BLC_DIGITAL_OFFSET);
1106
1107 v4l2_i2c_subdev_init(&mt9p031->subdev, client, &mt9p031_subdev_ops);
1108 mt9p031->subdev.internal_ops = &mt9p031_subdev_internal_ops;
1109
1110 mt9p031->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1111 mt9p031->pad.flags = MEDIA_PAD_FL_SOURCE;
1112 ret = media_entity_pads_init(&mt9p031->subdev.entity, 1, &mt9p031->pad);
1113 if (ret < 0)
1114 goto done;
1115
1116 mt9p031->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1117
1118 mt9p031->crop.width = MT9P031_WINDOW_WIDTH_DEF;
1119 mt9p031->crop.height = MT9P031_WINDOW_HEIGHT_DEF;
1120 mt9p031->crop.left = MT9P031_COLUMN_START_DEF;
1121 mt9p031->crop.top = MT9P031_ROW_START_DEF;
1122
1123 if (mt9p031->model == MT9P031_MODEL_MONOCHROME)
1124 mt9p031->format.code = MEDIA_BUS_FMT_Y12_1X12;
1125 else
1126 mt9p031->format.code = MEDIA_BUS_FMT_SGRBG12_1X12;
1127
1128 mt9p031->format.width = MT9P031_WINDOW_WIDTH_DEF;
1129 mt9p031->format.height = MT9P031_WINDOW_HEIGHT_DEF;
1130 mt9p031->format.field = V4L2_FIELD_NONE;
1131 mt9p031->format.colorspace = V4L2_COLORSPACE_SRGB;
1132
1133 mt9p031->reset = devm_gpiod_get_optional(&client->dev, "reset",
1134 GPIOD_OUT_HIGH);
1135
1136 ret = mt9p031_clk_setup(mt9p031);
1137 if (ret)
1138 goto done;
1139
1140 ret = v4l2_async_register_subdev(&mt9p031->subdev);
1141
1142done:
1143 if (ret < 0) {
1144 v4l2_ctrl_handler_free(&mt9p031->ctrls);
1145 media_entity_cleanup(&mt9p031->subdev.entity);
1146 mutex_destroy(&mt9p031->power_lock);
1147 }
1148
1149 return ret;
1150}
1151
1152static int mt9p031_remove(struct i2c_client *client)
1153{
1154 struct v4l2_subdev *subdev = i2c_get_clientdata(client);
1155 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
1156
1157 v4l2_ctrl_handler_free(&mt9p031->ctrls);
1158 v4l2_async_unregister_subdev(subdev);
1159 media_entity_cleanup(&subdev->entity);
1160 mutex_destroy(&mt9p031->power_lock);
1161
1162 return 0;
1163}
1164
1165static const struct i2c_device_id mt9p031_id[] = {
1166 { "mt9p031", MT9P031_MODEL_COLOR },
1167 { "mt9p031m", MT9P031_MODEL_MONOCHROME },
1168 { }
1169};
1170MODULE_DEVICE_TABLE(i2c, mt9p031_id);
1171
1172#if IS_ENABLED(CONFIG_OF)
1173static const struct of_device_id mt9p031_of_match[] = {
1174 { .compatible = "aptina,mt9p031", },
1175 { .compatible = "aptina,mt9p031m", },
1176 { },
1177};
1178MODULE_DEVICE_TABLE(of, mt9p031_of_match);
1179#endif
1180
1181static struct i2c_driver mt9p031_i2c_driver = {
1182 .driver = {
1183 .of_match_table = of_match_ptr(mt9p031_of_match),
1184 .name = "mt9p031",
1185 },
1186 .probe = mt9p031_probe,
1187 .remove = mt9p031_remove,
1188 .id_table = mt9p031_id,
1189};
1190
1191module_i2c_driver(mt9p031_i2c_driver);
1192
1193MODULE_DESCRIPTION("Aptina MT9P031 Camera driver");
1194MODULE_AUTHOR("Bastian Hecht <hechtb@gmail.com>");
1195MODULE_LICENSE("GPL v2");
1196
