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/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-fwnode.h>
31 #include <media/v4l2-subdev.h>
32
33 #include "aptina-pll.h"
34
35 #define MT9P031_PIXEL_ARRAY_WIDTH 2752
36 #define MT9P031_PIXEL_ARRAY_HEIGHT 2004
37
38 #define MT9P031_CHIP_VERSION 0x00
39 #define MT9P031_CHIP_VERSION_VALUE 0x1801
40 #define MT9P031_ROW_START 0x01
41 #define MT9P031_ROW_START_MIN 0
42 #define MT9P031_ROW_START_MAX 2004
43 #define MT9P031_ROW_START_DEF 54
44 #define MT9P031_COLUMN_START 0x02
45 #define MT9P031_COLUMN_START_MIN 0
46 #define MT9P031_COLUMN_START_MAX 2750
47 #define MT9P031_COLUMN_START_DEF 16
48 #define MT9P031_WINDOW_HEIGHT 0x03
49 #define MT9P031_WINDOW_HEIGHT_MIN 2
50 #define MT9P031_WINDOW_HEIGHT_MAX 2006
51 #define MT9P031_WINDOW_HEIGHT_DEF 1944
52 #define MT9P031_WINDOW_WIDTH 0x04
53 #define MT9P031_WINDOW_WIDTH_MIN 2
54 #define MT9P031_WINDOW_WIDTH_MAX 2752
55 #define MT9P031_WINDOW_WIDTH_DEF 2592
56 #define MT9P031_HORIZONTAL_BLANK 0x05
57 #define MT9P031_HORIZONTAL_BLANK_MIN 0
58 #define MT9P031_HORIZONTAL_BLANK_MAX 4095
59 #define MT9P031_VERTICAL_BLANK 0x06
60 #define MT9P031_VERTICAL_BLANK_MIN 1
61 #define MT9P031_VERTICAL_BLANK_MAX 4096
62 #define MT9P031_VERTICAL_BLANK_DEF 26
63 #define MT9P031_OUTPUT_CONTROL 0x07
64 #define MT9P031_OUTPUT_CONTROL_CEN 2
65 #define MT9P031_OUTPUT_CONTROL_SYN 1
66 #define MT9P031_OUTPUT_CONTROL_DEF 0x1f82
67 #define MT9P031_SHUTTER_WIDTH_UPPER 0x08
68 #define MT9P031_SHUTTER_WIDTH_LOWER 0x09
69 #define MT9P031_SHUTTER_WIDTH_MIN 1
70 #define MT9P031_SHUTTER_WIDTH_MAX 1048575
71 #define MT9P031_SHUTTER_WIDTH_DEF 1943
72 #define MT9P031_PLL_CONTROL 0x10
73 #define MT9P031_PLL_CONTROL_PWROFF 0x0050
74 #define MT9P031_PLL_CONTROL_PWRON 0x0051
75 #define MT9P031_PLL_CONTROL_USEPLL 0x0052
76 #define MT9P031_PLL_CONFIG_1 0x11
77 #define MT9P031_PLL_CONFIG_2 0x12
78 #define MT9P031_PIXEL_CLOCK_CONTROL 0x0a
79 #define MT9P031_PIXEL_CLOCK_INVERT BIT(15)
80 #define MT9P031_PIXEL_CLOCK_SHIFT(n) ((n) << 8)
81 #define MT9P031_PIXEL_CLOCK_DIVIDE(n) ((n) << 0)
82 #define MT9P031_RESTART 0x0b
83 #define MT9P031_FRAME_PAUSE_RESTART BIT(1)
84 #define MT9P031_FRAME_RESTART BIT(0)
85 #define MT9P031_SHUTTER_DELAY 0x0c
86 #define MT9P031_RST 0x0d
87 #define MT9P031_RST_ENABLE BIT(0)
88 #define MT9P031_READ_MODE_1 0x1e
89 #define MT9P031_READ_MODE_2 0x20
90 #define MT9P031_READ_MODE_2_ROW_MIR BIT(15)
91 #define MT9P031_READ_MODE_2_COL_MIR BIT(14)
92 #define MT9P031_READ_MODE_2_ROW_BLC BIT(6)
93 #define MT9P031_ROW_ADDRESS_MODE 0x22
94 #define MT9P031_COLUMN_ADDRESS_MODE 0x23
95 #define MT9P031_GLOBAL_GAIN 0x35
96 #define MT9P031_GLOBAL_GAIN_MIN 8
97 #define MT9P031_GLOBAL_GAIN_MAX 1024
98 #define MT9P031_GLOBAL_GAIN_DEF 8
99 #define MT9P031_GLOBAL_GAIN_MULT BIT(6)
100 #define MT9P031_ROW_BLACK_TARGET 0x49
101 #define MT9P031_ROW_BLACK_DEF_OFFSET 0x4b
102 #define MT9P031_GREEN1_OFFSET 0x60
103 #define MT9P031_GREEN2_OFFSET 0x61
104 #define MT9P031_BLACK_LEVEL_CALIBRATION 0x62
105 #define MT9P031_BLC_MANUAL_BLC BIT(0)
106 #define MT9P031_RED_OFFSET 0x63
107 #define MT9P031_BLUE_OFFSET 0x64
108 #define MT9P031_TEST_PATTERN 0xa0
109 #define MT9P031_TEST_PATTERN_SHIFT 3
110 #define MT9P031_TEST_PATTERN_ENABLE BIT(0)
111 #define MT9P031_TEST_PATTERN_GREEN 0xa1
112 #define MT9P031_TEST_PATTERN_RED 0xa2
113 #define MT9P031_TEST_PATTERN_BLUE 0xa3
114
115 enum mt9p031_model {
116 MT9P031_MODEL_COLOR,
117 MT9P031_MODEL_MONOCHROME,
118 };
119
120 struct mt9p031 {
121 struct v4l2_subdev subdev;
122 struct media_pad pad;
123 struct v4l2_rect crop; /* Sensor window */
124 struct v4l2_mbus_framefmt format;
125 struct mt9p031_platform_data *pdata;
126 struct mutex power_lock; /* lock to protect power_count */
127 int power_count;
128
129 struct clk *clk;
130 struct regulator_bulk_data regulators[3];
131
132 enum mt9p031_model model;
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 struct mt9p031_platform_data *pdata = mt9p031->pdata;
233 unsigned long ext_freq;
234 int ret;
235
236 mt9p031->clk = devm_clk_get(&client->dev, NULL);
237 if (IS_ERR(mt9p031->clk))
238 return PTR_ERR(mt9p031->clk);
239
240 ret = clk_set_rate(mt9p031->clk, pdata->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, pdata->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 = pdata->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 int ret;
311
312 /* Ensure RESET_BAR is active */
313 if (mt9p031->reset) {
314 gpiod_set_value(mt9p031->reset, 1);
315 usleep_range(1000, 2000);
316 }
317
318 /* Bring up the supplies */
319 ret = regulator_bulk_enable(ARRAY_SIZE(mt9p031->regulators),
320 mt9p031->regulators);
321 if (ret < 0)
322 return ret;
323
324 /* Enable clock */
325 if (mt9p031->clk) {
326 ret = clk_prepare_enable(mt9p031->clk);
327 if (ret) {
328 regulator_bulk_disable(ARRAY_SIZE(mt9p031->regulators),
329 mt9p031->regulators);
330 return ret;
331 }
332 }
333
334 /* Now RESET_BAR must be high */
335 if (mt9p031->reset) {
336 gpiod_set_value(mt9p031->reset, 0);
337 usleep_range(1000, 2000);
338 }
339
340 return 0;
341 }
342
mt9p031_power_off(struct mt9p031 * mt9p031)343 static void mt9p031_power_off(struct mt9p031 *mt9p031)
344 {
345 if (mt9p031->reset) {
346 gpiod_set_value(mt9p031->reset, 1);
347 usleep_range(1000, 2000);
348 }
349
350 regulator_bulk_disable(ARRAY_SIZE(mt9p031->regulators),
351 mt9p031->regulators);
352
353 clk_disable_unprepare(mt9p031->clk);
354 }
355
__mt9p031_set_power(struct mt9p031 * mt9p031,bool on)356 static int __mt9p031_set_power(struct mt9p031 *mt9p031, bool on)
357 {
358 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
359 int ret;
360
361 if (!on) {
362 mt9p031_power_off(mt9p031);
363 return 0;
364 }
365
366 ret = mt9p031_power_on(mt9p031);
367 if (ret < 0)
368 return ret;
369
370 ret = mt9p031_reset(mt9p031);
371 if (ret < 0) {
372 dev_err(&client->dev, "Failed to reset the camera\n");
373 return ret;
374 }
375
376 /* Configure the pixel clock polarity */
377 if (mt9p031->pdata && mt9p031->pdata->pixclk_pol) {
378 ret = mt9p031_write(client, MT9P031_PIXEL_CLOCK_CONTROL,
379 MT9P031_PIXEL_CLOCK_INVERT);
380 if (ret < 0)
381 return ret;
382 }
383
384 return v4l2_ctrl_handler_setup(&mt9p031->ctrls);
385 }
386
387 /* -----------------------------------------------------------------------------
388 * V4L2 subdev video operations
389 */
390
mt9p031_set_params(struct mt9p031 * mt9p031)391 static int mt9p031_set_params(struct mt9p031 *mt9p031)
392 {
393 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
394 struct v4l2_mbus_framefmt *format = &mt9p031->format;
395 const struct v4l2_rect *crop = &mt9p031->crop;
396 unsigned int hblank;
397 unsigned int vblank;
398 unsigned int xskip;
399 unsigned int yskip;
400 unsigned int xbin;
401 unsigned int ybin;
402 int ret;
403
404 /* Windows position and size.
405 *
406 * TODO: Make sure the start coordinates and window size match the
407 * skipping, binning and mirroring (see description of registers 2 and 4
408 * in table 13, and Binning section on page 41).
409 */
410 ret = mt9p031_write(client, MT9P031_COLUMN_START, crop->left);
411 if (ret < 0)
412 return ret;
413 ret = mt9p031_write(client, MT9P031_ROW_START, crop->top);
414 if (ret < 0)
415 return ret;
416 ret = mt9p031_write(client, MT9P031_WINDOW_WIDTH, crop->width - 1);
417 if (ret < 0)
418 return ret;
419 ret = mt9p031_write(client, MT9P031_WINDOW_HEIGHT, crop->height - 1);
420 if (ret < 0)
421 return ret;
422
423 /* Row and column binning and skipping. Use the maximum binning value
424 * compatible with the skipping settings.
425 */
426 xskip = DIV_ROUND_CLOSEST(crop->width, format->width);
427 yskip = DIV_ROUND_CLOSEST(crop->height, format->height);
428 xbin = 1 << (ffs(xskip) - 1);
429 ybin = 1 << (ffs(yskip) - 1);
430
431 ret = mt9p031_write(client, MT9P031_COLUMN_ADDRESS_MODE,
432 ((xbin - 1) << 4) | (xskip - 1));
433 if (ret < 0)
434 return ret;
435 ret = mt9p031_write(client, MT9P031_ROW_ADDRESS_MODE,
436 ((ybin - 1) << 4) | (yskip - 1));
437 if (ret < 0)
438 return ret;
439
440 /* Blanking - use minimum value for horizontal blanking and default
441 * value for vertical blanking.
442 */
443 hblank = 346 * ybin + 64 + (80 >> min_t(unsigned int, xbin, 3));
444 vblank = MT9P031_VERTICAL_BLANK_DEF;
445
446 ret = mt9p031_write(client, MT9P031_HORIZONTAL_BLANK, hblank - 1);
447 if (ret < 0)
448 return ret;
449 ret = mt9p031_write(client, MT9P031_VERTICAL_BLANK, vblank - 1);
450 if (ret < 0)
451 return ret;
452
453 return ret;
454 }
455
mt9p031_s_stream(struct v4l2_subdev * subdev,int enable)456 static int mt9p031_s_stream(struct v4l2_subdev *subdev, int enable)
457 {
458 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
459 struct i2c_client *client = v4l2_get_subdevdata(subdev);
460 int val;
461 int ret;
462
463 if (!enable) {
464 /* enable pause restart */
465 val = MT9P031_FRAME_PAUSE_RESTART;
466 ret = mt9p031_write(client, MT9P031_RESTART, val);
467 if (ret < 0)
468 return ret;
469
470 /* enable restart + keep pause restart set */
471 val |= MT9P031_FRAME_RESTART;
472 ret = mt9p031_write(client, MT9P031_RESTART, val);
473 if (ret < 0)
474 return ret;
475
476 /* Stop sensor readout */
477 ret = mt9p031_set_output_control(mt9p031,
478 MT9P031_OUTPUT_CONTROL_CEN, 0);
479 if (ret < 0)
480 return ret;
481
482 return mt9p031_pll_disable(mt9p031);
483 }
484
485 ret = mt9p031_set_params(mt9p031);
486 if (ret < 0)
487 return ret;
488
489 /* Switch to master "normal" mode */
490 ret = mt9p031_set_output_control(mt9p031, 0,
491 MT9P031_OUTPUT_CONTROL_CEN);
492 if (ret < 0)
493 return ret;
494
495 /*
496 * - clear pause restart
497 * - don't clear restart as clearing restart manually can cause
498 * undefined behavior
499 */
500 val = MT9P031_FRAME_RESTART;
501 ret = mt9p031_write(client, MT9P031_RESTART, val);
502 if (ret < 0)
503 return ret;
504
505 return mt9p031_pll_enable(mt9p031);
506 }
507
mt9p031_enum_mbus_code(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_mbus_code_enum * code)508 static int mt9p031_enum_mbus_code(struct v4l2_subdev *subdev,
509 struct v4l2_subdev_state *sd_state,
510 struct v4l2_subdev_mbus_code_enum *code)
511 {
512 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
513
514 if (code->pad || code->index)
515 return -EINVAL;
516
517 code->code = mt9p031->format.code;
518 return 0;
519 }
520
mt9p031_enum_frame_size(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_frame_size_enum * fse)521 static int mt9p031_enum_frame_size(struct v4l2_subdev *subdev,
522 struct v4l2_subdev_state *sd_state,
523 struct v4l2_subdev_frame_size_enum *fse)
524 {
525 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
526
527 if (fse->index >= 8 || fse->code != mt9p031->format.code)
528 return -EINVAL;
529
530 fse->min_width = MT9P031_WINDOW_WIDTH_DEF
531 / min_t(unsigned int, 7, fse->index + 1);
532 fse->max_width = fse->min_width;
533 fse->min_height = MT9P031_WINDOW_HEIGHT_DEF / (fse->index + 1);
534 fse->max_height = fse->min_height;
535
536 return 0;
537 }
538
539 static struct v4l2_mbus_framefmt *
__mt9p031_get_pad_format(struct mt9p031 * mt9p031,struct v4l2_subdev_state * sd_state,unsigned int pad,u32 which)540 __mt9p031_get_pad_format(struct mt9p031 *mt9p031,
541 struct v4l2_subdev_state *sd_state,
542 unsigned int pad, u32 which)
543 {
544 switch (which) {
545 case V4L2_SUBDEV_FORMAT_TRY:
546 return v4l2_subdev_get_try_format(&mt9p031->subdev, sd_state,
547 pad);
548 case V4L2_SUBDEV_FORMAT_ACTIVE:
549 return &mt9p031->format;
550 default:
551 return NULL;
552 }
553 }
554
555 static struct v4l2_rect *
__mt9p031_get_pad_crop(struct mt9p031 * mt9p031,struct v4l2_subdev_state * sd_state,unsigned int pad,u32 which)556 __mt9p031_get_pad_crop(struct mt9p031 *mt9p031,
557 struct v4l2_subdev_state *sd_state,
558 unsigned int pad, u32 which)
559 {
560 switch (which) {
561 case V4L2_SUBDEV_FORMAT_TRY:
562 return v4l2_subdev_get_try_crop(&mt9p031->subdev, sd_state,
563 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_cfg(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state)695 static int mt9p031_init_cfg(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 v4l2_subdev_get_try_crop(subdev, sd_state, 0);
706 crop->left = MT9P031_COLUMN_START_DEF;
707 crop->top = MT9P031_ROW_START_DEF;
708 crop->width = MT9P031_WINDOW_WIDTH_DEF;
709 crop->height = MT9P031_WINDOW_HEIGHT_DEF;
710
711 format = __mt9p031_get_pad_format(mt9p031, sd_state, 0, which);
712
713 if (mt9p031->model == MT9P031_MODEL_MONOCHROME)
714 format->code = MEDIA_BUS_FMT_Y12_1X12;
715 else
716 format->code = MEDIA_BUS_FMT_SGRBG12_1X12;
717
718 format->width = MT9P031_WINDOW_WIDTH_DEF;
719 format->height = MT9P031_WINDOW_HEIGHT_DEF;
720 format->field = V4L2_FIELD_NONE;
721 format->colorspace = V4L2_COLORSPACE_SRGB;
722
723 return 0;
724 }
725
726 /* -----------------------------------------------------------------------------
727 * V4L2 subdev control operations
728 */
729
730 #define V4L2_CID_BLC_AUTO (V4L2_CID_USER_BASE | 0x1002)
731 #define V4L2_CID_BLC_TARGET_LEVEL (V4L2_CID_USER_BASE | 0x1003)
732 #define V4L2_CID_BLC_ANALOG_OFFSET (V4L2_CID_USER_BASE | 0x1004)
733 #define V4L2_CID_BLC_DIGITAL_OFFSET (V4L2_CID_USER_BASE | 0x1005)
734
mt9p031_restore_blc(struct mt9p031 * mt9p031)735 static int mt9p031_restore_blc(struct mt9p031 *mt9p031)
736 {
737 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
738 int ret;
739
740 if (mt9p031->blc_auto->cur.val != 0) {
741 ret = mt9p031_set_mode2(mt9p031, 0,
742 MT9P031_READ_MODE_2_ROW_BLC);
743 if (ret < 0)
744 return ret;
745 }
746
747 if (mt9p031->blc_offset->cur.val != 0) {
748 ret = mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
749 mt9p031->blc_offset->cur.val);
750 if (ret < 0)
751 return ret;
752 }
753
754 return 0;
755 }
756
mt9p031_s_ctrl(struct v4l2_ctrl * ctrl)757 static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
758 {
759 struct mt9p031 *mt9p031 =
760 container_of(ctrl->handler, struct mt9p031, ctrls);
761 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
762 u16 data;
763 int ret;
764
765 if (ctrl->flags & V4L2_CTRL_FLAG_INACTIVE)
766 return 0;
767
768 switch (ctrl->id) {
769 case V4L2_CID_EXPOSURE:
770 ret = mt9p031_write(client, MT9P031_SHUTTER_WIDTH_UPPER,
771 (ctrl->val >> 16) & 0xffff);
772 if (ret < 0)
773 return ret;
774
775 return mt9p031_write(client, MT9P031_SHUTTER_WIDTH_LOWER,
776 ctrl->val & 0xffff);
777
778 case V4L2_CID_GAIN:
779 /* Gain is controlled by 2 analog stages and a digital stage.
780 * Valid values for the 3 stages are
781 *
782 * Stage Min Max Step
783 * ------------------------------------------
784 * First analog stage x1 x2 1
785 * Second analog stage x1 x4 0.125
786 * Digital stage x1 x16 0.125
787 *
788 * To minimize noise, the gain stages should be used in the
789 * second analog stage, first analog stage, digital stage order.
790 * Gain from a previous stage should be pushed to its maximum
791 * value before the next stage is used.
792 */
793 if (ctrl->val <= 32) {
794 data = ctrl->val;
795 } else if (ctrl->val <= 64) {
796 ctrl->val &= ~1;
797 data = (1 << 6) | (ctrl->val >> 1);
798 } else {
799 ctrl->val &= ~7;
800 data = ((ctrl->val - 64) << 5) | (1 << 6) | 32;
801 }
802
803 return mt9p031_write(client, MT9P031_GLOBAL_GAIN, data);
804
805 case V4L2_CID_HFLIP:
806 if (ctrl->val)
807 return mt9p031_set_mode2(mt9p031,
808 0, MT9P031_READ_MODE_2_COL_MIR);
809 else
810 return mt9p031_set_mode2(mt9p031,
811 MT9P031_READ_MODE_2_COL_MIR, 0);
812
813 case V4L2_CID_VFLIP:
814 if (ctrl->val)
815 return mt9p031_set_mode2(mt9p031,
816 0, MT9P031_READ_MODE_2_ROW_MIR);
817 else
818 return mt9p031_set_mode2(mt9p031,
819 MT9P031_READ_MODE_2_ROW_MIR, 0);
820
821 case V4L2_CID_TEST_PATTERN:
822 /* The digital side of the Black Level Calibration function must
823 * be disabled when generating a test pattern to avoid artifacts
824 * in the image. Activate (deactivate) the BLC-related controls
825 * when the test pattern is enabled (disabled).
826 */
827 v4l2_ctrl_activate(mt9p031->blc_auto, ctrl->val == 0);
828 v4l2_ctrl_activate(mt9p031->blc_offset, ctrl->val == 0);
829
830 if (!ctrl->val) {
831 /* Restore the BLC settings. */
832 ret = mt9p031_restore_blc(mt9p031);
833 if (ret < 0)
834 return ret;
835
836 return mt9p031_write(client, MT9P031_TEST_PATTERN, 0);
837 }
838
839 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_GREEN, 0x05a0);
840 if (ret < 0)
841 return ret;
842 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_RED, 0x0a50);
843 if (ret < 0)
844 return ret;
845 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_BLUE, 0x0aa0);
846 if (ret < 0)
847 return ret;
848
849 /* Disable digital BLC when generating a test pattern. */
850 ret = mt9p031_set_mode2(mt9p031, MT9P031_READ_MODE_2_ROW_BLC,
851 0);
852 if (ret < 0)
853 return ret;
854
855 ret = mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET, 0);
856 if (ret < 0)
857 return ret;
858
859 return mt9p031_write(client, MT9P031_TEST_PATTERN,
860 ((ctrl->val - 1) << MT9P031_TEST_PATTERN_SHIFT)
861 | MT9P031_TEST_PATTERN_ENABLE);
862
863 case V4L2_CID_BLC_AUTO:
864 ret = mt9p031_set_mode2(mt9p031,
865 ctrl->val ? 0 : MT9P031_READ_MODE_2_ROW_BLC,
866 ctrl->val ? MT9P031_READ_MODE_2_ROW_BLC : 0);
867 if (ret < 0)
868 return ret;
869
870 return mt9p031_write(client, MT9P031_BLACK_LEVEL_CALIBRATION,
871 ctrl->val ? 0 : MT9P031_BLC_MANUAL_BLC);
872
873 case V4L2_CID_BLC_TARGET_LEVEL:
874 return mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
875 ctrl->val);
876
877 case V4L2_CID_BLC_ANALOG_OFFSET:
878 data = ctrl->val & ((1 << 9) - 1);
879
880 ret = mt9p031_write(client, MT9P031_GREEN1_OFFSET, data);
881 if (ret < 0)
882 return ret;
883 ret = mt9p031_write(client, MT9P031_GREEN2_OFFSET, data);
884 if (ret < 0)
885 return ret;
886 ret = mt9p031_write(client, MT9P031_RED_OFFSET, data);
887 if (ret < 0)
888 return ret;
889 return mt9p031_write(client, MT9P031_BLUE_OFFSET, data);
890
891 case V4L2_CID_BLC_DIGITAL_OFFSET:
892 return mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET,
893 ctrl->val & ((1 << 12) - 1));
894 }
895
896 return 0;
897 }
898
899 static const struct v4l2_ctrl_ops mt9p031_ctrl_ops = {
900 .s_ctrl = mt9p031_s_ctrl,
901 };
902
903 static const char * const mt9p031_test_pattern_menu[] = {
904 "Disabled",
905 "Color Field",
906 "Horizontal Gradient",
907 "Vertical Gradient",
908 "Diagonal Gradient",
909 "Classic Test Pattern",
910 "Walking 1s",
911 "Monochrome Horizontal Bars",
912 "Monochrome Vertical Bars",
913 "Vertical Color Bars",
914 };
915
916 static const struct v4l2_ctrl_config mt9p031_ctrls[] = {
917 {
918 .ops = &mt9p031_ctrl_ops,
919 .id = V4L2_CID_BLC_AUTO,
920 .type = V4L2_CTRL_TYPE_BOOLEAN,
921 .name = "BLC, Auto",
922 .min = 0,
923 .max = 1,
924 .step = 1,
925 .def = 1,
926 .flags = 0,
927 }, {
928 .ops = &mt9p031_ctrl_ops,
929 .id = V4L2_CID_BLC_TARGET_LEVEL,
930 .type = V4L2_CTRL_TYPE_INTEGER,
931 .name = "BLC Target Level",
932 .min = 0,
933 .max = 4095,
934 .step = 1,
935 .def = 168,
936 .flags = 0,
937 }, {
938 .ops = &mt9p031_ctrl_ops,
939 .id = V4L2_CID_BLC_ANALOG_OFFSET,
940 .type = V4L2_CTRL_TYPE_INTEGER,
941 .name = "BLC Analog Offset",
942 .min = -255,
943 .max = 255,
944 .step = 1,
945 .def = 32,
946 .flags = 0,
947 }, {
948 .ops = &mt9p031_ctrl_ops,
949 .id = V4L2_CID_BLC_DIGITAL_OFFSET,
950 .type = V4L2_CTRL_TYPE_INTEGER,
951 .name = "BLC Digital Offset",
952 .min = -2048,
953 .max = 2047,
954 .step = 1,
955 .def = 40,
956 .flags = 0,
957 }
958 };
959
960 /* -----------------------------------------------------------------------------
961 * V4L2 subdev core operations
962 */
963
mt9p031_set_power(struct v4l2_subdev * subdev,int on)964 static int mt9p031_set_power(struct v4l2_subdev *subdev, int on)
965 {
966 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
967 int ret = 0;
968
969 mutex_lock(&mt9p031->power_lock);
970
971 /* If the power count is modified from 0 to != 0 or from != 0 to 0,
972 * update the power state.
973 */
974 if (mt9p031->power_count == !on) {
975 ret = __mt9p031_set_power(mt9p031, !!on);
976 if (ret < 0)
977 goto out;
978 }
979
980 /* Update the power count. */
981 mt9p031->power_count += on ? 1 : -1;
982 WARN_ON(mt9p031->power_count < 0);
983
984 out:
985 mutex_unlock(&mt9p031->power_lock);
986 return ret;
987 }
988
989 /* -----------------------------------------------------------------------------
990 * V4L2 subdev internal operations
991 */
992
mt9p031_registered(struct v4l2_subdev * subdev)993 static int mt9p031_registered(struct v4l2_subdev *subdev)
994 {
995 struct i2c_client *client = v4l2_get_subdevdata(subdev);
996 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
997 s32 data;
998 int ret;
999
1000 ret = mt9p031_power_on(mt9p031);
1001 if (ret < 0) {
1002 dev_err(&client->dev, "MT9P031 power up failed\n");
1003 return ret;
1004 }
1005
1006 /* Read out the chip version register */
1007 data = mt9p031_read(client, MT9P031_CHIP_VERSION);
1008 mt9p031_power_off(mt9p031);
1009
1010 if (data != MT9P031_CHIP_VERSION_VALUE) {
1011 dev_err(&client->dev, "MT9P031 not detected, wrong version "
1012 "0x%04x\n", data);
1013 return -ENODEV;
1014 }
1015
1016 dev_info(&client->dev, "MT9P031 detected at address 0x%02x\n",
1017 client->addr);
1018
1019 return 0;
1020 }
1021
mt9p031_open(struct v4l2_subdev * subdev,struct v4l2_subdev_fh * fh)1022 static int mt9p031_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
1023 {
1024 return mt9p031_set_power(subdev, 1);
1025 }
1026
mt9p031_close(struct v4l2_subdev * subdev,struct v4l2_subdev_fh * fh)1027 static int mt9p031_close(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
1028 {
1029 return mt9p031_set_power(subdev, 0);
1030 }
1031
1032 static const struct v4l2_subdev_core_ops mt9p031_subdev_core_ops = {
1033 .s_power = mt9p031_set_power,
1034 };
1035
1036 static const struct v4l2_subdev_video_ops mt9p031_subdev_video_ops = {
1037 .s_stream = mt9p031_s_stream,
1038 };
1039
1040 static const struct v4l2_subdev_pad_ops mt9p031_subdev_pad_ops = {
1041 .init_cfg = mt9p031_init_cfg,
1042 .enum_mbus_code = mt9p031_enum_mbus_code,
1043 .enum_frame_size = mt9p031_enum_frame_size,
1044 .get_fmt = mt9p031_get_format,
1045 .set_fmt = mt9p031_set_format,
1046 .get_selection = mt9p031_get_selection,
1047 .set_selection = mt9p031_set_selection,
1048 };
1049
1050 static const struct v4l2_subdev_ops mt9p031_subdev_ops = {
1051 .core = &mt9p031_subdev_core_ops,
1052 .video = &mt9p031_subdev_video_ops,
1053 .pad = &mt9p031_subdev_pad_ops,
1054 };
1055
1056 static const struct v4l2_subdev_internal_ops mt9p031_subdev_internal_ops = {
1057 .registered = mt9p031_registered,
1058 .open = mt9p031_open,
1059 .close = mt9p031_close,
1060 };
1061
1062 /* -----------------------------------------------------------------------------
1063 * Driver initialization and probing
1064 */
1065
1066 static struct mt9p031_platform_data *
mt9p031_get_pdata(struct i2c_client * client)1067 mt9p031_get_pdata(struct i2c_client *client)
1068 {
1069 struct mt9p031_platform_data *pdata = NULL;
1070 struct device_node *np;
1071 struct v4l2_fwnode_endpoint endpoint = {
1072 .bus_type = V4L2_MBUS_PARALLEL
1073 };
1074
1075 if (!IS_ENABLED(CONFIG_OF) || !client->dev.of_node)
1076 return client->dev.platform_data;
1077
1078 np = of_graph_get_next_endpoint(client->dev.of_node, NULL);
1079 if (!np)
1080 return NULL;
1081
1082 if (v4l2_fwnode_endpoint_parse(of_fwnode_handle(np), &endpoint) < 0)
1083 goto done;
1084
1085 pdata = devm_kzalloc(&client->dev, sizeof(*pdata), GFP_KERNEL);
1086 if (!pdata)
1087 goto done;
1088
1089 of_property_read_u32(np, "input-clock-frequency", &pdata->ext_freq);
1090 of_property_read_u32(np, "pixel-clock-frequency", &pdata->target_freq);
1091
1092 pdata->pixclk_pol = !!(endpoint.bus.parallel.flags &
1093 V4L2_MBUS_PCLK_SAMPLE_RISING);
1094
1095 done:
1096 of_node_put(np);
1097 return pdata;
1098 }
1099
mt9p031_probe(struct i2c_client * client,const struct i2c_device_id * did)1100 static int mt9p031_probe(struct i2c_client *client,
1101 const struct i2c_device_id *did)
1102 {
1103 struct mt9p031_platform_data *pdata = mt9p031_get_pdata(client);
1104 struct i2c_adapter *adapter = client->adapter;
1105 struct mt9p031 *mt9p031;
1106 unsigned int i;
1107 int ret;
1108
1109 if (pdata == NULL) {
1110 dev_err(&client->dev, "No platform data\n");
1111 return -EINVAL;
1112 }
1113
1114 if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
1115 dev_warn(&client->dev,
1116 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
1117 return -EIO;
1118 }
1119
1120 mt9p031 = devm_kzalloc(&client->dev, sizeof(*mt9p031), GFP_KERNEL);
1121 if (mt9p031 == NULL)
1122 return -ENOMEM;
1123
1124 mt9p031->pdata = pdata;
1125 mt9p031->output_control = MT9P031_OUTPUT_CONTROL_DEF;
1126 mt9p031->mode2 = MT9P031_READ_MODE_2_ROW_BLC;
1127 mt9p031->model = did->driver_data;
1128
1129 mt9p031->regulators[0].supply = "vdd";
1130 mt9p031->regulators[1].supply = "vdd_io";
1131 mt9p031->regulators[2].supply = "vaa";
1132
1133 ret = devm_regulator_bulk_get(&client->dev, 3, mt9p031->regulators);
1134 if (ret < 0) {
1135 dev_err(&client->dev, "Unable to get regulators\n");
1136 return ret;
1137 }
1138
1139 mutex_init(&mt9p031->power_lock);
1140
1141 v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 6);
1142
1143 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1144 V4L2_CID_EXPOSURE, MT9P031_SHUTTER_WIDTH_MIN,
1145 MT9P031_SHUTTER_WIDTH_MAX, 1,
1146 MT9P031_SHUTTER_WIDTH_DEF);
1147 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1148 V4L2_CID_GAIN, MT9P031_GLOBAL_GAIN_MIN,
1149 MT9P031_GLOBAL_GAIN_MAX, 1, MT9P031_GLOBAL_GAIN_DEF);
1150 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1151 V4L2_CID_HFLIP, 0, 1, 1, 0);
1152 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1153 V4L2_CID_VFLIP, 0, 1, 1, 0);
1154 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1155 V4L2_CID_PIXEL_RATE, pdata->target_freq,
1156 pdata->target_freq, 1, pdata->target_freq);
1157 v4l2_ctrl_new_std_menu_items(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1158 V4L2_CID_TEST_PATTERN,
1159 ARRAY_SIZE(mt9p031_test_pattern_menu) - 1, 0,
1160 0, mt9p031_test_pattern_menu);
1161
1162 for (i = 0; i < ARRAY_SIZE(mt9p031_ctrls); ++i)
1163 v4l2_ctrl_new_custom(&mt9p031->ctrls, &mt9p031_ctrls[i], NULL);
1164
1165 mt9p031->subdev.ctrl_handler = &mt9p031->ctrls;
1166
1167 if (mt9p031->ctrls.error) {
1168 printk(KERN_INFO "%s: control initialization error %d\n",
1169 __func__, mt9p031->ctrls.error);
1170 ret = mt9p031->ctrls.error;
1171 goto done;
1172 }
1173
1174 mt9p031->blc_auto = v4l2_ctrl_find(&mt9p031->ctrls, V4L2_CID_BLC_AUTO);
1175 mt9p031->blc_offset = v4l2_ctrl_find(&mt9p031->ctrls,
1176 V4L2_CID_BLC_DIGITAL_OFFSET);
1177
1178 v4l2_i2c_subdev_init(&mt9p031->subdev, client, &mt9p031_subdev_ops);
1179 mt9p031->subdev.internal_ops = &mt9p031_subdev_internal_ops;
1180
1181 mt9p031->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1182 mt9p031->pad.flags = MEDIA_PAD_FL_SOURCE;
1183 ret = media_entity_pads_init(&mt9p031->subdev.entity, 1, &mt9p031->pad);
1184 if (ret < 0)
1185 goto done;
1186
1187 mt9p031->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1188
1189 ret = mt9p031_init_cfg(&mt9p031->subdev, NULL);
1190 if (ret)
1191 goto done;
1192
1193 mt9p031->reset = devm_gpiod_get_optional(&client->dev, "reset",
1194 GPIOD_OUT_HIGH);
1195
1196 ret = mt9p031_clk_setup(mt9p031);
1197 if (ret)
1198 goto done;
1199
1200 ret = v4l2_async_register_subdev(&mt9p031->subdev);
1201
1202 done:
1203 if (ret < 0) {
1204 v4l2_ctrl_handler_free(&mt9p031->ctrls);
1205 media_entity_cleanup(&mt9p031->subdev.entity);
1206 mutex_destroy(&mt9p031->power_lock);
1207 }
1208
1209 return ret;
1210 }
1211
mt9p031_remove(struct i2c_client * client)1212 static void mt9p031_remove(struct i2c_client *client)
1213 {
1214 struct v4l2_subdev *subdev = i2c_get_clientdata(client);
1215 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
1216
1217 v4l2_ctrl_handler_free(&mt9p031->ctrls);
1218 v4l2_async_unregister_subdev(subdev);
1219 media_entity_cleanup(&subdev->entity);
1220 mutex_destroy(&mt9p031->power_lock);
1221 }
1222
1223 static const struct i2c_device_id mt9p031_id[] = {
1224 { "mt9p006", MT9P031_MODEL_COLOR },
1225 { "mt9p031", MT9P031_MODEL_COLOR },
1226 { "mt9p031m", MT9P031_MODEL_MONOCHROME },
1227 { }
1228 };
1229 MODULE_DEVICE_TABLE(i2c, mt9p031_id);
1230
1231 #if IS_ENABLED(CONFIG_OF)
1232 static const struct of_device_id mt9p031_of_match[] = {
1233 { .compatible = "aptina,mt9p006", },
1234 { .compatible = "aptina,mt9p031", },
1235 { .compatible = "aptina,mt9p031m", },
1236 { /* sentinel */ },
1237 };
1238 MODULE_DEVICE_TABLE(of, mt9p031_of_match);
1239 #endif
1240
1241 static struct i2c_driver mt9p031_i2c_driver = {
1242 .driver = {
1243 .of_match_table = of_match_ptr(mt9p031_of_match),
1244 .name = "mt9p031",
1245 },
1246 .probe = mt9p031_probe,
1247 .remove = mt9p031_remove,
1248 .id_table = mt9p031_id,
1249 };
1250
1251 module_i2c_driver(mt9p031_i2c_driver);
1252
1253 MODULE_DESCRIPTION("Aptina MT9P031 Camera driver");
1254 MODULE_AUTHOR("Bastian Hecht <hechtb@gmail.com>");
1255 MODULE_LICENSE("GPL v2");
1256
