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