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/module.h>
19 #include <linux/pm.h>
20 #include <linux/property.h>
21 #include <linux/regulator/consumer.h>
22 #include <linux/slab.h>
23 #include <linux/videodev2.h>
24
25 #include <media/v4l2-async.h>
26 #include <media/v4l2-ctrls.h>
27 #include <media/v4l2-device.h>
28 #include <media/v4l2-fwnode.h>
29 #include <media/v4l2-subdev.h>
30
31 #include "aptina-pll.h"
32
33 #define MT9P031_PIXEL_ARRAY_WIDTH 2752
34 #define MT9P031_PIXEL_ARRAY_HEIGHT 2004
35
36 #define MT9P031_CHIP_VERSION 0x00
37 #define MT9P031_CHIP_VERSION_VALUE 0x1801
38 #define MT9P031_ROW_START 0x01
39 #define MT9P031_ROW_START_MIN 0
40 #define MT9P031_ROW_START_MAX 2004
41 #define MT9P031_ROW_START_DEF 54
42 #define MT9P031_COLUMN_START 0x02
43 #define MT9P031_COLUMN_START_MIN 0
44 #define MT9P031_COLUMN_START_MAX 2750
45 #define MT9P031_COLUMN_START_DEF 16
46 #define MT9P031_WINDOW_HEIGHT 0x03
47 #define MT9P031_WINDOW_HEIGHT_MIN 2
48 #define MT9P031_WINDOW_HEIGHT_MAX 2006
49 #define MT9P031_WINDOW_HEIGHT_DEF 1944
50 #define MT9P031_WINDOW_WIDTH 0x04
51 #define MT9P031_WINDOW_WIDTH_MIN 2
52 #define MT9P031_WINDOW_WIDTH_MAX 2752
53 #define MT9P031_WINDOW_WIDTH_DEF 2592
54 #define MT9P031_HORIZONTAL_BLANK 0x05
55 #define MT9P031_HORIZONTAL_BLANK_MIN 0
56 #define MT9P031_HORIZONTAL_BLANK_MAX 4095
57 #define MT9P031_VERTICAL_BLANK 0x06
58 #define MT9P031_VERTICAL_BLANK_MIN 1
59 #define MT9P031_VERTICAL_BLANK_MAX 4096
60 #define MT9P031_VERTICAL_BLANK_DEF 26
61 #define MT9P031_OUTPUT_CONTROL 0x07
62 #define MT9P031_OUTPUT_CONTROL_CEN 2
63 #define MT9P031_OUTPUT_CONTROL_SYN 1
64 #define MT9P031_OUTPUT_CONTROL_DEF 0x1f82
65 #define MT9P031_SHUTTER_WIDTH_UPPER 0x08
66 #define MT9P031_SHUTTER_WIDTH_LOWER 0x09
67 #define MT9P031_SHUTTER_WIDTH_MIN 1
68 #define MT9P031_SHUTTER_WIDTH_MAX 1048575
69 #define MT9P031_SHUTTER_WIDTH_DEF 1943
70 #define MT9P031_PLL_CONTROL 0x10
71 #define MT9P031_PLL_CONTROL_PWROFF 0x0050
72 #define MT9P031_PLL_CONTROL_PWRON 0x0051
73 #define MT9P031_PLL_CONTROL_USEPLL 0x0052
74 #define MT9P031_PLL_CONFIG_1 0x11
75 #define MT9P031_PLL_CONFIG_2 0x12
76 #define MT9P031_PIXEL_CLOCK_CONTROL 0x0a
77 #define MT9P031_PIXEL_CLOCK_INVERT BIT(15)
78 #define MT9P031_PIXEL_CLOCK_SHIFT(n) ((n) << 8)
79 #define MT9P031_PIXEL_CLOCK_DIVIDE(n) ((n) << 0)
80 #define MT9P031_RESTART 0x0b
81 #define MT9P031_FRAME_PAUSE_RESTART BIT(1)
82 #define MT9P031_FRAME_RESTART BIT(0)
83 #define MT9P031_SHUTTER_DELAY 0x0c
84 #define MT9P031_RST 0x0d
85 #define MT9P031_RST_ENABLE BIT(0)
86 #define MT9P031_READ_MODE_1 0x1e
87 #define MT9P031_READ_MODE_2 0x20
88 #define MT9P031_READ_MODE_2_ROW_MIR BIT(15)
89 #define MT9P031_READ_MODE_2_COL_MIR BIT(14)
90 #define MT9P031_READ_MODE_2_ROW_BLC BIT(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 BIT(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 BIT(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 BIT(0)
109 #define MT9P031_TEST_PATTERN_GREEN 0xa1
110 #define MT9P031_TEST_PATTERN_RED 0xa2
111 #define MT9P031_TEST_PATTERN_BLUE 0xa3
112
113 struct mt9p031_model_info {
114 u32 code;
115 };
116
117 struct mt9p031 {
118 struct v4l2_subdev subdev;
119 struct media_pad pad;
120 struct v4l2_rect crop; /* Sensor window */
121 struct v4l2_mbus_framefmt format;
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 unsigned int pixclk_pol:1;
129 int ext_freq;
130 int target_freq;
131
132 u32 code;
133 struct aptina_pll pll;
134 unsigned int clk_div;
135 bool use_pll;
136 struct gpio_desc *reset;
137
138 struct v4l2_ctrl_handler ctrls;
139 struct v4l2_ctrl *blc_auto;
140 struct v4l2_ctrl *blc_offset;
141
142 /* Registers cache */
143 u16 output_control;
144 u16 mode2;
145 };
146
to_mt9p031(struct v4l2_subdev * sd)147 static struct mt9p031 *to_mt9p031(struct v4l2_subdev *sd)
148 {
149 return container_of(sd, struct mt9p031, subdev);
150 }
151
mt9p031_read(struct i2c_client * client,u8 reg)152 static int mt9p031_read(struct i2c_client *client, u8 reg)
153 {
154 return i2c_smbus_read_word_swapped(client, reg);
155 }
156
mt9p031_write(struct i2c_client * client,u8 reg,u16 data)157 static int mt9p031_write(struct i2c_client *client, u8 reg, u16 data)
158 {
159 return i2c_smbus_write_word_swapped(client, reg, data);
160 }
161
mt9p031_set_output_control(struct mt9p031 * mt9p031,u16 clear,u16 set)162 static int mt9p031_set_output_control(struct mt9p031 *mt9p031, u16 clear,
163 u16 set)
164 {
165 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
166 u16 value = (mt9p031->output_control & ~clear) | set;
167 int ret;
168
169 ret = mt9p031_write(client, MT9P031_OUTPUT_CONTROL, value);
170 if (ret < 0)
171 return ret;
172
173 mt9p031->output_control = value;
174 return 0;
175 }
176
mt9p031_set_mode2(struct mt9p031 * mt9p031,u16 clear,u16 set)177 static int mt9p031_set_mode2(struct mt9p031 *mt9p031, u16 clear, u16 set)
178 {
179 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
180 u16 value = (mt9p031->mode2 & ~clear) | set;
181 int ret;
182
183 ret = mt9p031_write(client, MT9P031_READ_MODE_2, value);
184 if (ret < 0)
185 return ret;
186
187 mt9p031->mode2 = value;
188 return 0;
189 }
190
mt9p031_reset(struct mt9p031 * mt9p031)191 static int mt9p031_reset(struct mt9p031 *mt9p031)
192 {
193 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
194 int ret;
195
196 /* Disable chip output, synchronous option update */
197 ret = mt9p031_write(client, MT9P031_RST, MT9P031_RST_ENABLE);
198 if (ret < 0)
199 return ret;
200 ret = mt9p031_write(client, MT9P031_RST, 0);
201 if (ret < 0)
202 return ret;
203
204 ret = mt9p031_write(client, MT9P031_PIXEL_CLOCK_CONTROL,
205 MT9P031_PIXEL_CLOCK_DIVIDE(mt9p031->clk_div));
206 if (ret < 0)
207 return ret;
208
209 return mt9p031_set_output_control(mt9p031, MT9P031_OUTPUT_CONTROL_CEN,
210 0);
211 }
212
mt9p031_clk_setup(struct mt9p031 * mt9p031)213 static int mt9p031_clk_setup(struct mt9p031 *mt9p031)
214 {
215 static const struct aptina_pll_limits limits = {
216 .ext_clock_min = 6000000,
217 .ext_clock_max = 27000000,
218 .int_clock_min = 2000000,
219 .int_clock_max = 13500000,
220 .out_clock_min = 180000000,
221 .out_clock_max = 360000000,
222 .pix_clock_max = 96000000,
223 .n_min = 1,
224 .n_max = 64,
225 .m_min = 16,
226 .m_max = 255,
227 .p1_min = 1,
228 .p1_max = 128,
229 };
230
231 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
232 unsigned long ext_freq;
233 int ret;
234
235 mt9p031->clk = devm_v4l2_sensor_clk_get(&client->dev, NULL);
236 if (IS_ERR(mt9p031->clk))
237 return dev_err_probe(&client->dev, PTR_ERR(mt9p031->clk),
238 "failed to get the clock\n");
239
240 ret = clk_set_rate(mt9p031->clk, mt9p031->ext_freq);
241 if (ret < 0)
242 return ret;
243
244 ext_freq = clk_get_rate(mt9p031->clk);
245
246 /* If the external clock frequency is out of bounds for the PLL use the
247 * pixel clock divider only and disable the PLL.
248 */
249 if (ext_freq > limits.ext_clock_max) {
250 unsigned int div;
251
252 div = DIV_ROUND_UP(ext_freq, mt9p031->target_freq);
253 div = roundup_pow_of_two(div) / 2;
254
255 mt9p031->clk_div = min_t(unsigned int, div, 64);
256 mt9p031->use_pll = false;
257
258 return 0;
259 }
260
261 mt9p031->pll.ext_clock = ext_freq;
262 mt9p031->pll.pix_clock = mt9p031->target_freq;
263 mt9p031->use_pll = true;
264
265 return aptina_pll_calculate(&client->dev, &limits, &mt9p031->pll);
266 }
267
mt9p031_pll_enable(struct mt9p031 * mt9p031)268 static int mt9p031_pll_enable(struct mt9p031 *mt9p031)
269 {
270 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
271 int ret;
272
273 if (!mt9p031->use_pll)
274 return 0;
275
276 ret = mt9p031_write(client, MT9P031_PLL_CONTROL,
277 MT9P031_PLL_CONTROL_PWRON);
278 if (ret < 0)
279 return ret;
280
281 ret = mt9p031_write(client, MT9P031_PLL_CONFIG_1,
282 (mt9p031->pll.m << 8) | (mt9p031->pll.n - 1));
283 if (ret < 0)
284 return ret;
285
286 ret = mt9p031_write(client, MT9P031_PLL_CONFIG_2, mt9p031->pll.p1 - 1);
287 if (ret < 0)
288 return ret;
289
290 usleep_range(1000, 2000);
291 ret = mt9p031_write(client, MT9P031_PLL_CONTROL,
292 MT9P031_PLL_CONTROL_PWRON |
293 MT9P031_PLL_CONTROL_USEPLL);
294 return ret;
295 }
296
mt9p031_pll_disable(struct mt9p031 * mt9p031)297 static inline int mt9p031_pll_disable(struct mt9p031 *mt9p031)
298 {
299 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
300
301 if (!mt9p031->use_pll)
302 return 0;
303
304 return mt9p031_write(client, MT9P031_PLL_CONTROL,
305 MT9P031_PLL_CONTROL_PWROFF);
306 }
307
mt9p031_power_on(struct mt9p031 * mt9p031)308 static int mt9p031_power_on(struct mt9p031 *mt9p031)
309 {
310 unsigned long rate, delay;
311 int ret;
312
313 /* Ensure RESET_BAR is active */
314 if (mt9p031->reset) {
315 gpiod_set_value(mt9p031->reset, 1);
316 usleep_range(1000, 2000);
317 }
318
319 /* Bring up the supplies */
320 ret = regulator_bulk_enable(ARRAY_SIZE(mt9p031->regulators),
321 mt9p031->regulators);
322 if (ret < 0)
323 return ret;
324
325 /* Enable clock */
326 if (mt9p031->clk) {
327 ret = clk_prepare_enable(mt9p031->clk);
328 if (ret) {
329 regulator_bulk_disable(ARRAY_SIZE(mt9p031->regulators),
330 mt9p031->regulators);
331 return ret;
332 }
333 }
334
335 /* Now RESET_BAR must be high */
336 if (mt9p031->reset) {
337 gpiod_set_value(mt9p031->reset, 0);
338 /* Wait 850000 EXTCLK cycles before de-asserting reset. */
339 rate = clk_get_rate(mt9p031->clk);
340 if (!rate)
341 rate = 6000000; /* Slowest supported clock, 6 MHz */
342 delay = DIV_ROUND_UP(850000 * 1000, rate);
343 msleep(delay);
344 }
345
346 return 0;
347 }
348
mt9p031_power_off(struct mt9p031 * mt9p031)349 static void mt9p031_power_off(struct mt9p031 *mt9p031)
350 {
351 if (mt9p031->reset) {
352 gpiod_set_value(mt9p031->reset, 1);
353 usleep_range(1000, 2000);
354 }
355
356 regulator_bulk_disable(ARRAY_SIZE(mt9p031->regulators),
357 mt9p031->regulators);
358
359 clk_disable_unprepare(mt9p031->clk);
360 }
361
__mt9p031_set_power(struct mt9p031 * mt9p031,bool on)362 static int __mt9p031_set_power(struct mt9p031 *mt9p031, bool on)
363 {
364 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
365 int ret;
366
367 if (!on) {
368 mt9p031_power_off(mt9p031);
369 return 0;
370 }
371
372 ret = mt9p031_power_on(mt9p031);
373 if (ret < 0)
374 return ret;
375
376 ret = mt9p031_reset(mt9p031);
377 if (ret < 0) {
378 dev_err(&client->dev, "Failed to reset the camera\n");
379 return ret;
380 }
381
382 /* Configure the pixel clock polarity */
383 if (mt9p031->pixclk_pol) {
384 ret = mt9p031_write(client, MT9P031_PIXEL_CLOCK_CONTROL,
385 MT9P031_PIXEL_CLOCK_INVERT);
386 if (ret < 0)
387 return ret;
388 }
389
390 return v4l2_ctrl_handler_setup(&mt9p031->ctrls);
391 }
392
393 /* -----------------------------------------------------------------------------
394 * V4L2 subdev video operations
395 */
396
mt9p031_set_params(struct mt9p031 * mt9p031)397 static int mt9p031_set_params(struct mt9p031 *mt9p031)
398 {
399 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
400 struct v4l2_mbus_framefmt *format = &mt9p031->format;
401 const struct v4l2_rect *crop = &mt9p031->crop;
402 unsigned int hblank;
403 unsigned int vblank;
404 unsigned int xskip;
405 unsigned int yskip;
406 unsigned int xbin;
407 unsigned int ybin;
408 int ret;
409
410 /* Windows position and size.
411 *
412 * TODO: Make sure the start coordinates and window size match the
413 * skipping, binning and mirroring (see description of registers 2 and 4
414 * in table 13, and Binning section on page 41).
415 */
416 ret = mt9p031_write(client, MT9P031_COLUMN_START, crop->left);
417 if (ret < 0)
418 return ret;
419 ret = mt9p031_write(client, MT9P031_ROW_START, crop->top);
420 if (ret < 0)
421 return ret;
422 ret = mt9p031_write(client, MT9P031_WINDOW_WIDTH, crop->width - 1);
423 if (ret < 0)
424 return ret;
425 ret = mt9p031_write(client, MT9P031_WINDOW_HEIGHT, crop->height - 1);
426 if (ret < 0)
427 return ret;
428
429 /* Row and column binning and skipping. Use the maximum binning value
430 * compatible with the skipping settings.
431 */
432 xskip = DIV_ROUND_CLOSEST(crop->width, format->width);
433 yskip = DIV_ROUND_CLOSEST(crop->height, format->height);
434 xbin = 1 << (ffs(xskip) - 1);
435 ybin = 1 << (ffs(yskip) - 1);
436
437 ret = mt9p031_write(client, MT9P031_COLUMN_ADDRESS_MODE,
438 ((xbin - 1) << 4) | (xskip - 1));
439 if (ret < 0)
440 return ret;
441 ret = mt9p031_write(client, MT9P031_ROW_ADDRESS_MODE,
442 ((ybin - 1) << 4) | (yskip - 1));
443 if (ret < 0)
444 return ret;
445
446 /* Blanking - use minimum value for horizontal blanking and default
447 * value for vertical blanking.
448 */
449 hblank = 346 * ybin + 64 + (80 >> min_t(unsigned int, xbin, 3));
450 vblank = MT9P031_VERTICAL_BLANK_DEF;
451
452 ret = mt9p031_write(client, MT9P031_HORIZONTAL_BLANK, hblank - 1);
453 if (ret < 0)
454 return ret;
455 return mt9p031_write(client, MT9P031_VERTICAL_BLANK, vblank - 1);
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) >> 3) & 0x7f;
795 data = (data << 8) | (1 << 6) | 32;
796 }
797
798 return mt9p031_write(client, MT9P031_GLOBAL_GAIN, data);
799
800 case V4L2_CID_HFLIP:
801 if (ctrl->val)
802 return mt9p031_set_mode2(mt9p031,
803 0, MT9P031_READ_MODE_2_COL_MIR);
804 else
805 return mt9p031_set_mode2(mt9p031,
806 MT9P031_READ_MODE_2_COL_MIR, 0);
807
808 case V4L2_CID_VFLIP:
809 if (ctrl->val)
810 return mt9p031_set_mode2(mt9p031,
811 0, MT9P031_READ_MODE_2_ROW_MIR);
812 else
813 return mt9p031_set_mode2(mt9p031,
814 MT9P031_READ_MODE_2_ROW_MIR, 0);
815
816 case V4L2_CID_TEST_PATTERN:
817 /* The digital side of the Black Level Calibration function must
818 * be disabled when generating a test pattern to avoid artifacts
819 * in the image. Activate (deactivate) the BLC-related controls
820 * when the test pattern is enabled (disabled).
821 */
822 v4l2_ctrl_activate(mt9p031->blc_auto, ctrl->val == 0);
823 v4l2_ctrl_activate(mt9p031->blc_offset, ctrl->val == 0);
824
825 if (!ctrl->val) {
826 /* Restore the BLC settings. */
827 ret = mt9p031_restore_blc(mt9p031);
828 if (ret < 0)
829 return ret;
830
831 return mt9p031_write(client, MT9P031_TEST_PATTERN, 0);
832 }
833
834 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_GREEN, 0x05a0);
835 if (ret < 0)
836 return ret;
837 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_RED, 0x0a50);
838 if (ret < 0)
839 return ret;
840 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_BLUE, 0x0aa0);
841 if (ret < 0)
842 return ret;
843
844 /* Disable digital BLC when generating a test pattern. */
845 ret = mt9p031_set_mode2(mt9p031, MT9P031_READ_MODE_2_ROW_BLC,
846 0);
847 if (ret < 0)
848 return ret;
849
850 ret = mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET, 0);
851 if (ret < 0)
852 return ret;
853
854 return mt9p031_write(client, MT9P031_TEST_PATTERN,
855 ((ctrl->val - 1) << MT9P031_TEST_PATTERN_SHIFT)
856 | MT9P031_TEST_PATTERN_ENABLE);
857
858 case V4L2_CID_BLC_AUTO:
859 ret = mt9p031_set_mode2(mt9p031,
860 ctrl->val ? 0 : MT9P031_READ_MODE_2_ROW_BLC,
861 ctrl->val ? MT9P031_READ_MODE_2_ROW_BLC : 0);
862 if (ret < 0)
863 return ret;
864
865 return mt9p031_write(client, MT9P031_BLACK_LEVEL_CALIBRATION,
866 ctrl->val ? 0 : MT9P031_BLC_MANUAL_BLC);
867
868 case V4L2_CID_BLC_TARGET_LEVEL:
869 return mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
870 ctrl->val);
871
872 case V4L2_CID_BLC_ANALOG_OFFSET:
873 data = ctrl->val & ((1 << 9) - 1);
874
875 ret = mt9p031_write(client, MT9P031_GREEN1_OFFSET, data);
876 if (ret < 0)
877 return ret;
878 ret = mt9p031_write(client, MT9P031_GREEN2_OFFSET, data);
879 if (ret < 0)
880 return ret;
881 ret = mt9p031_write(client, MT9P031_RED_OFFSET, data);
882 if (ret < 0)
883 return ret;
884 return mt9p031_write(client, MT9P031_BLUE_OFFSET, data);
885
886 case V4L2_CID_BLC_DIGITAL_OFFSET:
887 return mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET,
888 ctrl->val & ((1 << 12) - 1));
889 }
890
891 return 0;
892 }
893
894 static const struct v4l2_ctrl_ops mt9p031_ctrl_ops = {
895 .s_ctrl = mt9p031_s_ctrl,
896 };
897
898 static const char * const mt9p031_test_pattern_menu[] = {
899 "Disabled",
900 "Color Field",
901 "Horizontal Gradient",
902 "Vertical Gradient",
903 "Diagonal Gradient",
904 "Classic Test Pattern",
905 "Walking 1s",
906 "Monochrome Horizontal Bars",
907 "Monochrome Vertical Bars",
908 "Vertical Color Bars",
909 };
910
911 static const struct v4l2_ctrl_config mt9p031_ctrls[] = {
912 {
913 .ops = &mt9p031_ctrl_ops,
914 .id = V4L2_CID_BLC_AUTO,
915 .type = V4L2_CTRL_TYPE_BOOLEAN,
916 .name = "BLC, Auto",
917 .min = 0,
918 .max = 1,
919 .step = 1,
920 .def = 1,
921 .flags = 0,
922 }, {
923 .ops = &mt9p031_ctrl_ops,
924 .id = V4L2_CID_BLC_TARGET_LEVEL,
925 .type = V4L2_CTRL_TYPE_INTEGER,
926 .name = "BLC Target Level",
927 .min = 0,
928 .max = 4095,
929 .step = 1,
930 .def = 168,
931 .flags = 0,
932 }, {
933 .ops = &mt9p031_ctrl_ops,
934 .id = V4L2_CID_BLC_ANALOG_OFFSET,
935 .type = V4L2_CTRL_TYPE_INTEGER,
936 .name = "BLC Analog Offset",
937 .min = -255,
938 .max = 255,
939 .step = 1,
940 .def = 32,
941 .flags = 0,
942 }, {
943 .ops = &mt9p031_ctrl_ops,
944 .id = V4L2_CID_BLC_DIGITAL_OFFSET,
945 .type = V4L2_CTRL_TYPE_INTEGER,
946 .name = "BLC Digital Offset",
947 .min = -2048,
948 .max = 2047,
949 .step = 1,
950 .def = 40,
951 .flags = 0,
952 }
953 };
954
955 /* -----------------------------------------------------------------------------
956 * V4L2 subdev core operations
957 */
958
mt9p031_set_power(struct v4l2_subdev * subdev,int on)959 static int mt9p031_set_power(struct v4l2_subdev *subdev, int on)
960 {
961 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
962 int ret = 0;
963
964 mutex_lock(&mt9p031->power_lock);
965
966 /* If the power count is modified from 0 to != 0 or from != 0 to 0,
967 * update the power state.
968 */
969 if (mt9p031->power_count == !on) {
970 ret = __mt9p031_set_power(mt9p031, !!on);
971 if (ret < 0)
972 goto out;
973 }
974
975 /* Update the power count. */
976 mt9p031->power_count += on ? 1 : -1;
977 WARN_ON(mt9p031->power_count < 0);
978
979 out:
980 mutex_unlock(&mt9p031->power_lock);
981 return ret;
982 }
983
984 /* -----------------------------------------------------------------------------
985 * V4L2 subdev internal operations
986 */
987
mt9p031_registered(struct v4l2_subdev * subdev)988 static int mt9p031_registered(struct v4l2_subdev *subdev)
989 {
990 struct i2c_client *client = v4l2_get_subdevdata(subdev);
991 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
992 s32 data;
993 int ret;
994
995 ret = mt9p031_power_on(mt9p031);
996 if (ret < 0) {
997 dev_err(&client->dev, "MT9P031 power up failed\n");
998 return ret;
999 }
1000
1001 /* Read out the chip version register */
1002 data = mt9p031_read(client, MT9P031_CHIP_VERSION);
1003 mt9p031_power_off(mt9p031);
1004
1005 if (data != MT9P031_CHIP_VERSION_VALUE) {
1006 dev_err(&client->dev, "MT9P031 not detected, wrong version "
1007 "0x%04x\n", data);
1008 return -ENODEV;
1009 }
1010
1011 dev_info(&client->dev, "MT9P031 detected at address 0x%02x\n",
1012 client->addr);
1013
1014 return 0;
1015 }
1016
mt9p031_open(struct v4l2_subdev * subdev,struct v4l2_subdev_fh * fh)1017 static int mt9p031_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
1018 {
1019 return mt9p031_set_power(subdev, 1);
1020 }
1021
mt9p031_close(struct v4l2_subdev * subdev,struct v4l2_subdev_fh * fh)1022 static int mt9p031_close(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
1023 {
1024 return mt9p031_set_power(subdev, 0);
1025 }
1026
1027 static const struct v4l2_subdev_core_ops mt9p031_subdev_core_ops = {
1028 .s_power = mt9p031_set_power,
1029 };
1030
1031 static const struct v4l2_subdev_video_ops mt9p031_subdev_video_ops = {
1032 .s_stream = mt9p031_s_stream,
1033 };
1034
1035 static const struct v4l2_subdev_pad_ops mt9p031_subdev_pad_ops = {
1036 .enum_mbus_code = mt9p031_enum_mbus_code,
1037 .enum_frame_size = mt9p031_enum_frame_size,
1038 .get_fmt = mt9p031_get_format,
1039 .set_fmt = mt9p031_set_format,
1040 .get_selection = mt9p031_get_selection,
1041 .set_selection = mt9p031_set_selection,
1042 };
1043
1044 static const struct v4l2_subdev_ops mt9p031_subdev_ops = {
1045 .core = &mt9p031_subdev_core_ops,
1046 .video = &mt9p031_subdev_video_ops,
1047 .pad = &mt9p031_subdev_pad_ops,
1048 };
1049
1050 static const struct v4l2_subdev_internal_ops mt9p031_subdev_internal_ops = {
1051 .init_state = mt9p031_init_state,
1052 .registered = mt9p031_registered,
1053 .open = mt9p031_open,
1054 .close = mt9p031_close,
1055 };
1056
1057 /* -----------------------------------------------------------------------------
1058 * Driver initialization and probing
1059 */
1060
mt9p031_parse_properties(struct mt9p031 * mt9p031,struct device * dev)1061 static int mt9p031_parse_properties(struct mt9p031 *mt9p031, struct device *dev)
1062 {
1063 struct v4l2_fwnode_endpoint endpoint = {
1064 .bus_type = V4L2_MBUS_PARALLEL
1065 };
1066 struct fwnode_handle *np;
1067 int ret;
1068
1069 np = fwnode_graph_get_next_endpoint(dev_fwnode(dev), NULL);
1070 if (!np)
1071 return dev_err_probe(dev, -EINVAL, "endpoint node not found\n");
1072
1073 ret = v4l2_fwnode_endpoint_parse(np, &endpoint);
1074 fwnode_handle_put(np);
1075 if (ret)
1076 return dev_err_probe(dev, -EINVAL, "could not parse endpoint\n");
1077
1078 fwnode_property_read_u32(np, "input-clock-frequency",
1079 &mt9p031->ext_freq);
1080 fwnode_property_read_u32(np, "pixel-clock-frequency",
1081 &mt9p031->target_freq);
1082
1083 mt9p031->pixclk_pol = !!(endpoint.bus.parallel.flags &
1084 V4L2_MBUS_PCLK_SAMPLE_RISING);
1085
1086 return 0;
1087 }
1088
mt9p031_probe(struct i2c_client * client)1089 static int mt9p031_probe(struct i2c_client *client)
1090 {
1091 struct i2c_adapter *adapter = client->adapter;
1092 const struct mt9p031_model_info *info;
1093 struct mt9p031 *mt9p031;
1094 unsigned int i;
1095 int ret;
1096
1097 if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
1098 dev_warn(&client->dev,
1099 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
1100 return -EIO;
1101 }
1102
1103 mt9p031 = devm_kzalloc(&client->dev, sizeof(*mt9p031), GFP_KERNEL);
1104 if (mt9p031 == NULL)
1105 return -ENOMEM;
1106
1107 ret = mt9p031_parse_properties(mt9p031, &client->dev);
1108 if (ret)
1109 return ret;
1110
1111 mt9p031->output_control = MT9P031_OUTPUT_CONTROL_DEF;
1112 mt9p031->mode2 = MT9P031_READ_MODE_2_ROW_BLC;
1113 info = device_get_match_data(&client->dev);
1114 mt9p031->code = info->code;
1115
1116 mt9p031->regulators[0].supply = "vdd";
1117 mt9p031->regulators[1].supply = "vdd_io";
1118 mt9p031->regulators[2].supply = "vaa";
1119
1120 ret = devm_regulator_bulk_get(&client->dev, 3, mt9p031->regulators);
1121 if (ret < 0) {
1122 dev_err(&client->dev, "Unable to get regulators\n");
1123 return ret;
1124 }
1125
1126 mutex_init(&mt9p031->power_lock);
1127
1128 v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 6);
1129
1130 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1131 V4L2_CID_EXPOSURE, MT9P031_SHUTTER_WIDTH_MIN,
1132 MT9P031_SHUTTER_WIDTH_MAX, 1,
1133 MT9P031_SHUTTER_WIDTH_DEF);
1134 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1135 V4L2_CID_GAIN, MT9P031_GLOBAL_GAIN_MIN,
1136 MT9P031_GLOBAL_GAIN_MAX, 1, MT9P031_GLOBAL_GAIN_DEF);
1137 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1138 V4L2_CID_HFLIP, 0, 1, 1, 0);
1139 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1140 V4L2_CID_VFLIP, 0, 1, 1, 0);
1141 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1142 V4L2_CID_PIXEL_RATE, mt9p031->target_freq,
1143 mt9p031->target_freq, 1, mt9p031->target_freq);
1144 v4l2_ctrl_new_std_menu_items(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1145 V4L2_CID_TEST_PATTERN,
1146 ARRAY_SIZE(mt9p031_test_pattern_menu) - 1, 0,
1147 0, mt9p031_test_pattern_menu);
1148
1149 for (i = 0; i < ARRAY_SIZE(mt9p031_ctrls); ++i)
1150 v4l2_ctrl_new_custom(&mt9p031->ctrls, &mt9p031_ctrls[i], NULL);
1151
1152 mt9p031->subdev.ctrl_handler = &mt9p031->ctrls;
1153
1154 if (mt9p031->ctrls.error) {
1155 printk(KERN_INFO "%s: control initialization error %d\n",
1156 __func__, mt9p031->ctrls.error);
1157 ret = mt9p031->ctrls.error;
1158 goto done;
1159 }
1160
1161 mt9p031->blc_auto = v4l2_ctrl_find(&mt9p031->ctrls, V4L2_CID_BLC_AUTO);
1162 mt9p031->blc_offset = v4l2_ctrl_find(&mt9p031->ctrls,
1163 V4L2_CID_BLC_DIGITAL_OFFSET);
1164
1165 v4l2_i2c_subdev_init(&mt9p031->subdev, client, &mt9p031_subdev_ops);
1166 mt9p031->subdev.internal_ops = &mt9p031_subdev_internal_ops;
1167
1168 mt9p031->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1169 mt9p031->pad.flags = MEDIA_PAD_FL_SOURCE;
1170 ret = media_entity_pads_init(&mt9p031->subdev.entity, 1, &mt9p031->pad);
1171 if (ret < 0)
1172 goto done;
1173
1174 mt9p031->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1175
1176 ret = mt9p031_init_state(&mt9p031->subdev, NULL);
1177 if (ret)
1178 goto done;
1179
1180 mt9p031->reset = devm_gpiod_get_optional(&client->dev, "reset",
1181 GPIOD_OUT_HIGH);
1182 if (IS_ERR(mt9p031->reset)) {
1183 ret = PTR_ERR(mt9p031->reset);
1184 goto done;
1185 }
1186
1187 ret = mt9p031_clk_setup(mt9p031);
1188 if (ret)
1189 goto done;
1190
1191 ret = v4l2_async_register_subdev(&mt9p031->subdev);
1192
1193 done:
1194 if (ret < 0) {
1195 v4l2_ctrl_handler_free(&mt9p031->ctrls);
1196 media_entity_cleanup(&mt9p031->subdev.entity);
1197 mutex_destroy(&mt9p031->power_lock);
1198 }
1199
1200 return ret;
1201 }
1202
mt9p031_remove(struct i2c_client * client)1203 static void mt9p031_remove(struct i2c_client *client)
1204 {
1205 struct v4l2_subdev *subdev = i2c_get_clientdata(client);
1206 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
1207
1208 v4l2_ctrl_handler_free(&mt9p031->ctrls);
1209 v4l2_async_unregister_subdev(subdev);
1210 media_entity_cleanup(&subdev->entity);
1211 mutex_destroy(&mt9p031->power_lock);
1212 }
1213
1214 static const struct mt9p031_model_info mt9p031_models_bayer = {
1215 .code = MEDIA_BUS_FMT_SGRBG12_1X12
1216 };
1217
1218 static const struct mt9p031_model_info mt9p031_models_mono = {
1219 .code = MEDIA_BUS_FMT_Y12_1X12
1220 };
1221
1222 static const struct of_device_id mt9p031_of_match[] = {
1223 { .compatible = "aptina,mt9p006", .data = &mt9p031_models_bayer },
1224 { .compatible = "aptina,mt9p031", .data = &mt9p031_models_bayer },
1225 { .compatible = "aptina,mt9p031m", .data = &mt9p031_models_mono },
1226 { /* sentinel */ }
1227 };
1228 MODULE_DEVICE_TABLE(of, mt9p031_of_match);
1229
1230 static struct i2c_driver mt9p031_i2c_driver = {
1231 .driver = {
1232 .of_match_table = mt9p031_of_match,
1233 .name = "mt9p031",
1234 },
1235 .probe = mt9p031_probe,
1236 .remove = mt9p031_remove,
1237 };
1238
1239 module_i2c_driver(mt9p031_i2c_driver);
1240
1241 MODULE_DESCRIPTION("Aptina MT9P031 Camera driver");
1242 MODULE_AUTHOR("Bastian Hecht <hechtb@gmail.com>");
1243 MODULE_LICENSE("GPL v2");
1244