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 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->pdata && mt9p031->pdata->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 ret = mt9p031_write(client, MT9P031_VERTICAL_BLANK, vblank - 1);
456 if (ret < 0)
457 return ret;
458
459 return ret;
460 }
461
mt9p031_s_stream(struct v4l2_subdev * subdev,int enable)462 static int mt9p031_s_stream(struct v4l2_subdev *subdev, int enable)
463 {
464 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
465 struct i2c_client *client = v4l2_get_subdevdata(subdev);
466 int val;
467 int ret;
468
469 if (!enable) {
470 /* enable pause restart */
471 val = MT9P031_FRAME_PAUSE_RESTART;
472 ret = mt9p031_write(client, MT9P031_RESTART, val);
473 if (ret < 0)
474 return ret;
475
476 /* enable restart + keep pause restart set */
477 val |= MT9P031_FRAME_RESTART;
478 ret = mt9p031_write(client, MT9P031_RESTART, val);
479 if (ret < 0)
480 return ret;
481
482 /* Stop sensor readout */
483 ret = mt9p031_set_output_control(mt9p031,
484 MT9P031_OUTPUT_CONTROL_CEN, 0);
485 if (ret < 0)
486 return ret;
487
488 return mt9p031_pll_disable(mt9p031);
489 }
490
491 ret = mt9p031_set_params(mt9p031);
492 if (ret < 0)
493 return ret;
494
495 /* Switch to master "normal" mode */
496 ret = mt9p031_set_output_control(mt9p031, 0,
497 MT9P031_OUTPUT_CONTROL_CEN);
498 if (ret < 0)
499 return ret;
500
501 /*
502 * - clear pause restart
503 * - don't clear restart as clearing restart manually can cause
504 * undefined behavior
505 */
506 val = MT9P031_FRAME_RESTART;
507 ret = mt9p031_write(client, MT9P031_RESTART, val);
508 if (ret < 0)
509 return ret;
510
511 return mt9p031_pll_enable(mt9p031);
512 }
513
mt9p031_enum_mbus_code(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_mbus_code_enum * code)514 static int mt9p031_enum_mbus_code(struct v4l2_subdev *subdev,
515 struct v4l2_subdev_state *sd_state,
516 struct v4l2_subdev_mbus_code_enum *code)
517 {
518 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
519
520 if (code->pad || code->index)
521 return -EINVAL;
522
523 code->code = mt9p031->format.code;
524 return 0;
525 }
526
mt9p031_enum_frame_size(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_frame_size_enum * fse)527 static int mt9p031_enum_frame_size(struct v4l2_subdev *subdev,
528 struct v4l2_subdev_state *sd_state,
529 struct v4l2_subdev_frame_size_enum *fse)
530 {
531 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
532
533 if (fse->index >= 8 || fse->code != mt9p031->format.code)
534 return -EINVAL;
535
536 fse->min_width = MT9P031_WINDOW_WIDTH_DEF
537 / min_t(unsigned int, 7, fse->index + 1);
538 fse->max_width = fse->min_width;
539 fse->min_height = MT9P031_WINDOW_HEIGHT_DEF / (fse->index + 1);
540 fse->max_height = fse->min_height;
541
542 return 0;
543 }
544
545 static struct v4l2_mbus_framefmt *
__mt9p031_get_pad_format(struct mt9p031 * mt9p031,struct v4l2_subdev_state * sd_state,unsigned int pad,u32 which)546 __mt9p031_get_pad_format(struct mt9p031 *mt9p031,
547 struct v4l2_subdev_state *sd_state,
548 unsigned int pad, u32 which)
549 {
550 switch (which) {
551 case V4L2_SUBDEV_FORMAT_TRY:
552 return v4l2_subdev_get_try_format(&mt9p031->subdev, sd_state,
553 pad);
554 case V4L2_SUBDEV_FORMAT_ACTIVE:
555 return &mt9p031->format;
556 default:
557 return NULL;
558 }
559 }
560
561 static struct v4l2_rect *
__mt9p031_get_pad_crop(struct mt9p031 * mt9p031,struct v4l2_subdev_state * sd_state,unsigned int pad,u32 which)562 __mt9p031_get_pad_crop(struct mt9p031 *mt9p031,
563 struct v4l2_subdev_state *sd_state,
564 unsigned int pad, u32 which)
565 {
566 switch (which) {
567 case V4L2_SUBDEV_FORMAT_TRY:
568 return v4l2_subdev_get_try_crop(&mt9p031->subdev, sd_state,
569 pad);
570 case V4L2_SUBDEV_FORMAT_ACTIVE:
571 return &mt9p031->crop;
572 default:
573 return NULL;
574 }
575 }
576
mt9p031_get_format(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * fmt)577 static int mt9p031_get_format(struct v4l2_subdev *subdev,
578 struct v4l2_subdev_state *sd_state,
579 struct v4l2_subdev_format *fmt)
580 {
581 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
582
583 fmt->format = *__mt9p031_get_pad_format(mt9p031, sd_state, fmt->pad,
584 fmt->which);
585 return 0;
586 }
587
mt9p031_set_format(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * format)588 static int mt9p031_set_format(struct v4l2_subdev *subdev,
589 struct v4l2_subdev_state *sd_state,
590 struct v4l2_subdev_format *format)
591 {
592 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
593 struct v4l2_mbus_framefmt *__format;
594 struct v4l2_rect *__crop;
595 unsigned int width;
596 unsigned int height;
597 unsigned int hratio;
598 unsigned int vratio;
599
600 __crop = __mt9p031_get_pad_crop(mt9p031, sd_state, format->pad,
601 format->which);
602
603 /* Clamp the width and height to avoid dividing by zero. */
604 width = clamp_t(unsigned int, ALIGN(format->format.width, 2),
605 max_t(unsigned int, __crop->width / 7,
606 MT9P031_WINDOW_WIDTH_MIN),
607 __crop->width);
608 height = clamp_t(unsigned int, ALIGN(format->format.height, 2),
609 max_t(unsigned int, __crop->height / 8,
610 MT9P031_WINDOW_HEIGHT_MIN),
611 __crop->height);
612
613 hratio = DIV_ROUND_CLOSEST(__crop->width, width);
614 vratio = DIV_ROUND_CLOSEST(__crop->height, height);
615
616 __format = __mt9p031_get_pad_format(mt9p031, sd_state, format->pad,
617 format->which);
618 __format->width = __crop->width / hratio;
619 __format->height = __crop->height / vratio;
620
621 format->format = *__format;
622
623 return 0;
624 }
625
mt9p031_get_selection(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_selection * sel)626 static int mt9p031_get_selection(struct v4l2_subdev *subdev,
627 struct v4l2_subdev_state *sd_state,
628 struct v4l2_subdev_selection *sel)
629 {
630 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
631
632 switch (sel->target) {
633 case V4L2_SEL_TGT_CROP_BOUNDS:
634 sel->r.left = MT9P031_COLUMN_START_MIN;
635 sel->r.top = MT9P031_ROW_START_MIN;
636 sel->r.width = MT9P031_WINDOW_WIDTH_MAX;
637 sel->r.height = MT9P031_WINDOW_HEIGHT_MAX;
638 return 0;
639
640 case V4L2_SEL_TGT_CROP:
641 sel->r = *__mt9p031_get_pad_crop(mt9p031, sd_state,
642 sel->pad, sel->which);
643 return 0;
644
645 default:
646 return -EINVAL;
647 }
648 }
649
mt9p031_set_selection(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_selection * sel)650 static int mt9p031_set_selection(struct v4l2_subdev *subdev,
651 struct v4l2_subdev_state *sd_state,
652 struct v4l2_subdev_selection *sel)
653 {
654 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
655 struct v4l2_mbus_framefmt *__format;
656 struct v4l2_rect *__crop;
657 struct v4l2_rect rect;
658
659 if (sel->target != V4L2_SEL_TGT_CROP)
660 return -EINVAL;
661
662 /* Clamp the crop rectangle boundaries and align them to a multiple of 2
663 * pixels to ensure a GRBG Bayer pattern.
664 */
665 rect.left = clamp(ALIGN(sel->r.left, 2), MT9P031_COLUMN_START_MIN,
666 MT9P031_COLUMN_START_MAX);
667 rect.top = clamp(ALIGN(sel->r.top, 2), MT9P031_ROW_START_MIN,
668 MT9P031_ROW_START_MAX);
669 rect.width = clamp_t(unsigned int, ALIGN(sel->r.width, 2),
670 MT9P031_WINDOW_WIDTH_MIN,
671 MT9P031_WINDOW_WIDTH_MAX);
672 rect.height = clamp_t(unsigned int, ALIGN(sel->r.height, 2),
673 MT9P031_WINDOW_HEIGHT_MIN,
674 MT9P031_WINDOW_HEIGHT_MAX);
675
676 rect.width = min_t(unsigned int, rect.width,
677 MT9P031_PIXEL_ARRAY_WIDTH - rect.left);
678 rect.height = min_t(unsigned int, rect.height,
679 MT9P031_PIXEL_ARRAY_HEIGHT - rect.top);
680
681 __crop = __mt9p031_get_pad_crop(mt9p031, sd_state, sel->pad,
682 sel->which);
683
684 if (rect.width != __crop->width || rect.height != __crop->height) {
685 /* Reset the output image size if the crop rectangle size has
686 * been modified.
687 */
688 __format = __mt9p031_get_pad_format(mt9p031, sd_state,
689 sel->pad,
690 sel->which);
691 __format->width = rect.width;
692 __format->height = rect.height;
693 }
694
695 *__crop = rect;
696 sel->r = rect;
697
698 return 0;
699 }
700
mt9p031_init_cfg(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state)701 static int mt9p031_init_cfg(struct v4l2_subdev *subdev,
702 struct v4l2_subdev_state *sd_state)
703 {
704 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
705 struct v4l2_mbus_framefmt *format;
706 struct v4l2_rect *crop;
707 const int which = sd_state == NULL ? V4L2_SUBDEV_FORMAT_ACTIVE :
708 V4L2_SUBDEV_FORMAT_TRY;
709
710 crop = __mt9p031_get_pad_crop(mt9p031, sd_state, 0, which);
711 crop->left = MT9P031_COLUMN_START_DEF;
712 crop->top = MT9P031_ROW_START_DEF;
713 crop->width = MT9P031_WINDOW_WIDTH_DEF;
714 crop->height = MT9P031_WINDOW_HEIGHT_DEF;
715
716 format = __mt9p031_get_pad_format(mt9p031, sd_state, 0, which);
717
718 if (mt9p031->model == MT9P031_MODEL_MONOCHROME)
719 format->code = MEDIA_BUS_FMT_Y12_1X12;
720 else
721 format->code = MEDIA_BUS_FMT_SGRBG12_1X12;
722
723 format->width = MT9P031_WINDOW_WIDTH_DEF;
724 format->height = MT9P031_WINDOW_HEIGHT_DEF;
725 format->field = V4L2_FIELD_NONE;
726 format->colorspace = V4L2_COLORSPACE_SRGB;
727
728 return 0;
729 }
730
731 /* -----------------------------------------------------------------------------
732 * V4L2 subdev control operations
733 */
734
735 #define V4L2_CID_BLC_AUTO (V4L2_CID_USER_BASE | 0x1002)
736 #define V4L2_CID_BLC_TARGET_LEVEL (V4L2_CID_USER_BASE | 0x1003)
737 #define V4L2_CID_BLC_ANALOG_OFFSET (V4L2_CID_USER_BASE | 0x1004)
738 #define V4L2_CID_BLC_DIGITAL_OFFSET (V4L2_CID_USER_BASE | 0x1005)
739
mt9p031_restore_blc(struct mt9p031 * mt9p031)740 static int mt9p031_restore_blc(struct mt9p031 *mt9p031)
741 {
742 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
743 int ret;
744
745 if (mt9p031->blc_auto->cur.val != 0) {
746 ret = mt9p031_set_mode2(mt9p031, 0,
747 MT9P031_READ_MODE_2_ROW_BLC);
748 if (ret < 0)
749 return ret;
750 }
751
752 if (mt9p031->blc_offset->cur.val != 0) {
753 ret = mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
754 mt9p031->blc_offset->cur.val);
755 if (ret < 0)
756 return ret;
757 }
758
759 return 0;
760 }
761
mt9p031_s_ctrl(struct v4l2_ctrl * ctrl)762 static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
763 {
764 struct mt9p031 *mt9p031 =
765 container_of(ctrl->handler, struct mt9p031, ctrls);
766 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
767 u16 data;
768 int ret;
769
770 if (ctrl->flags & V4L2_CTRL_FLAG_INACTIVE)
771 return 0;
772
773 switch (ctrl->id) {
774 case V4L2_CID_EXPOSURE:
775 ret = mt9p031_write(client, MT9P031_SHUTTER_WIDTH_UPPER,
776 (ctrl->val >> 16) & 0xffff);
777 if (ret < 0)
778 return ret;
779
780 return mt9p031_write(client, MT9P031_SHUTTER_WIDTH_LOWER,
781 ctrl->val & 0xffff);
782
783 case V4L2_CID_GAIN:
784 /* Gain is controlled by 2 analog stages and a digital stage.
785 * Valid values for the 3 stages are
786 *
787 * Stage Min Max Step
788 * ------------------------------------------
789 * First analog stage x1 x2 1
790 * Second analog stage x1 x4 0.125
791 * Digital stage x1 x16 0.125
792 *
793 * To minimize noise, the gain stages should be used in the
794 * second analog stage, first analog stage, digital stage order.
795 * Gain from a previous stage should be pushed to its maximum
796 * value before the next stage is used.
797 */
798 if (ctrl->val <= 32) {
799 data = ctrl->val;
800 } else if (ctrl->val <= 64) {
801 ctrl->val &= ~1;
802 data = (1 << 6) | (ctrl->val >> 1);
803 } else {
804 ctrl->val &= ~7;
805 data = ((ctrl->val - 64) << 5) | (1 << 6) | 32;
806 }
807
808 return mt9p031_write(client, MT9P031_GLOBAL_GAIN, data);
809
810 case V4L2_CID_HFLIP:
811 if (ctrl->val)
812 return mt9p031_set_mode2(mt9p031,
813 0, MT9P031_READ_MODE_2_COL_MIR);
814 else
815 return mt9p031_set_mode2(mt9p031,
816 MT9P031_READ_MODE_2_COL_MIR, 0);
817
818 case V4L2_CID_VFLIP:
819 if (ctrl->val)
820 return mt9p031_set_mode2(mt9p031,
821 0, MT9P031_READ_MODE_2_ROW_MIR);
822 else
823 return mt9p031_set_mode2(mt9p031,
824 MT9P031_READ_MODE_2_ROW_MIR, 0);
825
826 case V4L2_CID_TEST_PATTERN:
827 /* The digital side of the Black Level Calibration function must
828 * be disabled when generating a test pattern to avoid artifacts
829 * in the image. Activate (deactivate) the BLC-related controls
830 * when the test pattern is enabled (disabled).
831 */
832 v4l2_ctrl_activate(mt9p031->blc_auto, ctrl->val == 0);
833 v4l2_ctrl_activate(mt9p031->blc_offset, ctrl->val == 0);
834
835 if (!ctrl->val) {
836 /* Restore the BLC settings. */
837 ret = mt9p031_restore_blc(mt9p031);
838 if (ret < 0)
839 return ret;
840
841 return mt9p031_write(client, MT9P031_TEST_PATTERN, 0);
842 }
843
844 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_GREEN, 0x05a0);
845 if (ret < 0)
846 return ret;
847 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_RED, 0x0a50);
848 if (ret < 0)
849 return ret;
850 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_BLUE, 0x0aa0);
851 if (ret < 0)
852 return ret;
853
854 /* Disable digital BLC when generating a test pattern. */
855 ret = mt9p031_set_mode2(mt9p031, MT9P031_READ_MODE_2_ROW_BLC,
856 0);
857 if (ret < 0)
858 return ret;
859
860 ret = mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET, 0);
861 if (ret < 0)
862 return ret;
863
864 return mt9p031_write(client, MT9P031_TEST_PATTERN,
865 ((ctrl->val - 1) << MT9P031_TEST_PATTERN_SHIFT)
866 | MT9P031_TEST_PATTERN_ENABLE);
867
868 case V4L2_CID_BLC_AUTO:
869 ret = mt9p031_set_mode2(mt9p031,
870 ctrl->val ? 0 : MT9P031_READ_MODE_2_ROW_BLC,
871 ctrl->val ? MT9P031_READ_MODE_2_ROW_BLC : 0);
872 if (ret < 0)
873 return ret;
874
875 return mt9p031_write(client, MT9P031_BLACK_LEVEL_CALIBRATION,
876 ctrl->val ? 0 : MT9P031_BLC_MANUAL_BLC);
877
878 case V4L2_CID_BLC_TARGET_LEVEL:
879 return mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
880 ctrl->val);
881
882 case V4L2_CID_BLC_ANALOG_OFFSET:
883 data = ctrl->val & ((1 << 9) - 1);
884
885 ret = mt9p031_write(client, MT9P031_GREEN1_OFFSET, data);
886 if (ret < 0)
887 return ret;
888 ret = mt9p031_write(client, MT9P031_GREEN2_OFFSET, data);
889 if (ret < 0)
890 return ret;
891 ret = mt9p031_write(client, MT9P031_RED_OFFSET, data);
892 if (ret < 0)
893 return ret;
894 return mt9p031_write(client, MT9P031_BLUE_OFFSET, data);
895
896 case V4L2_CID_BLC_DIGITAL_OFFSET:
897 return mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET,
898 ctrl->val & ((1 << 12) - 1));
899 }
900
901 return 0;
902 }
903
904 static const struct v4l2_ctrl_ops mt9p031_ctrl_ops = {
905 .s_ctrl = mt9p031_s_ctrl,
906 };
907
908 static const char * const mt9p031_test_pattern_menu[] = {
909 "Disabled",
910 "Color Field",
911 "Horizontal Gradient",
912 "Vertical Gradient",
913 "Diagonal Gradient",
914 "Classic Test Pattern",
915 "Walking 1s",
916 "Monochrome Horizontal Bars",
917 "Monochrome Vertical Bars",
918 "Vertical Color Bars",
919 };
920
921 static const struct v4l2_ctrl_config mt9p031_ctrls[] = {
922 {
923 .ops = &mt9p031_ctrl_ops,
924 .id = V4L2_CID_BLC_AUTO,
925 .type = V4L2_CTRL_TYPE_BOOLEAN,
926 .name = "BLC, Auto",
927 .min = 0,
928 .max = 1,
929 .step = 1,
930 .def = 1,
931 .flags = 0,
932 }, {
933 .ops = &mt9p031_ctrl_ops,
934 .id = V4L2_CID_BLC_TARGET_LEVEL,
935 .type = V4L2_CTRL_TYPE_INTEGER,
936 .name = "BLC Target Level",
937 .min = 0,
938 .max = 4095,
939 .step = 1,
940 .def = 168,
941 .flags = 0,
942 }, {
943 .ops = &mt9p031_ctrl_ops,
944 .id = V4L2_CID_BLC_ANALOG_OFFSET,
945 .type = V4L2_CTRL_TYPE_INTEGER,
946 .name = "BLC Analog Offset",
947 .min = -255,
948 .max = 255,
949 .step = 1,
950 .def = 32,
951 .flags = 0,
952 }, {
953 .ops = &mt9p031_ctrl_ops,
954 .id = V4L2_CID_BLC_DIGITAL_OFFSET,
955 .type = V4L2_CTRL_TYPE_INTEGER,
956 .name = "BLC Digital Offset",
957 .min = -2048,
958 .max = 2047,
959 .step = 1,
960 .def = 40,
961 .flags = 0,
962 }
963 };
964
965 /* -----------------------------------------------------------------------------
966 * V4L2 subdev core operations
967 */
968
mt9p031_set_power(struct v4l2_subdev * subdev,int on)969 static int mt9p031_set_power(struct v4l2_subdev *subdev, int on)
970 {
971 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
972 int ret = 0;
973
974 mutex_lock(&mt9p031->power_lock);
975
976 /* If the power count is modified from 0 to != 0 or from != 0 to 0,
977 * update the power state.
978 */
979 if (mt9p031->power_count == !on) {
980 ret = __mt9p031_set_power(mt9p031, !!on);
981 if (ret < 0)
982 goto out;
983 }
984
985 /* Update the power count. */
986 mt9p031->power_count += on ? 1 : -1;
987 WARN_ON(mt9p031->power_count < 0);
988
989 out:
990 mutex_unlock(&mt9p031->power_lock);
991 return ret;
992 }
993
994 /* -----------------------------------------------------------------------------
995 * V4L2 subdev internal operations
996 */
997
mt9p031_registered(struct v4l2_subdev * subdev)998 static int mt9p031_registered(struct v4l2_subdev *subdev)
999 {
1000 struct i2c_client *client = v4l2_get_subdevdata(subdev);
1001 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
1002 s32 data;
1003 int ret;
1004
1005 ret = mt9p031_power_on(mt9p031);
1006 if (ret < 0) {
1007 dev_err(&client->dev, "MT9P031 power up failed\n");
1008 return ret;
1009 }
1010
1011 /* Read out the chip version register */
1012 data = mt9p031_read(client, MT9P031_CHIP_VERSION);
1013 mt9p031_power_off(mt9p031);
1014
1015 if (data != MT9P031_CHIP_VERSION_VALUE) {
1016 dev_err(&client->dev, "MT9P031 not detected, wrong version "
1017 "0x%04x\n", data);
1018 return -ENODEV;
1019 }
1020
1021 dev_info(&client->dev, "MT9P031 detected at address 0x%02x\n",
1022 client->addr);
1023
1024 return 0;
1025 }
1026
mt9p031_open(struct v4l2_subdev * subdev,struct v4l2_subdev_fh * fh)1027 static int mt9p031_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
1028 {
1029 return mt9p031_set_power(subdev, 1);
1030 }
1031
mt9p031_close(struct v4l2_subdev * subdev,struct v4l2_subdev_fh * fh)1032 static int mt9p031_close(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
1033 {
1034 return mt9p031_set_power(subdev, 0);
1035 }
1036
1037 static const struct v4l2_subdev_core_ops mt9p031_subdev_core_ops = {
1038 .s_power = mt9p031_set_power,
1039 };
1040
1041 static const struct v4l2_subdev_video_ops mt9p031_subdev_video_ops = {
1042 .s_stream = mt9p031_s_stream,
1043 };
1044
1045 static const struct v4l2_subdev_pad_ops mt9p031_subdev_pad_ops = {
1046 .init_cfg = mt9p031_init_cfg,
1047 .enum_mbus_code = mt9p031_enum_mbus_code,
1048 .enum_frame_size = mt9p031_enum_frame_size,
1049 .get_fmt = mt9p031_get_format,
1050 .set_fmt = mt9p031_set_format,
1051 .get_selection = mt9p031_get_selection,
1052 .set_selection = mt9p031_set_selection,
1053 };
1054
1055 static const struct v4l2_subdev_ops mt9p031_subdev_ops = {
1056 .core = &mt9p031_subdev_core_ops,
1057 .video = &mt9p031_subdev_video_ops,
1058 .pad = &mt9p031_subdev_pad_ops,
1059 };
1060
1061 static const struct v4l2_subdev_internal_ops mt9p031_subdev_internal_ops = {
1062 .registered = mt9p031_registered,
1063 .open = mt9p031_open,
1064 .close = mt9p031_close,
1065 };
1066
1067 /* -----------------------------------------------------------------------------
1068 * Driver initialization and probing
1069 */
1070
1071 static struct mt9p031_platform_data *
mt9p031_get_pdata(struct i2c_client * client)1072 mt9p031_get_pdata(struct i2c_client *client)
1073 {
1074 struct mt9p031_platform_data *pdata = NULL;
1075 struct device_node *np;
1076 struct v4l2_fwnode_endpoint endpoint = {
1077 .bus_type = V4L2_MBUS_PARALLEL
1078 };
1079
1080 if (!IS_ENABLED(CONFIG_OF) || !client->dev.of_node)
1081 return client->dev.platform_data;
1082
1083 np = of_graph_get_next_endpoint(client->dev.of_node, NULL);
1084 if (!np)
1085 return NULL;
1086
1087 if (v4l2_fwnode_endpoint_parse(of_fwnode_handle(np), &endpoint) < 0)
1088 goto done;
1089
1090 pdata = devm_kzalloc(&client->dev, sizeof(*pdata), GFP_KERNEL);
1091 if (!pdata)
1092 goto done;
1093
1094 of_property_read_u32(np, "input-clock-frequency", &pdata->ext_freq);
1095 of_property_read_u32(np, "pixel-clock-frequency", &pdata->target_freq);
1096
1097 pdata->pixclk_pol = !!(endpoint.bus.parallel.flags &
1098 V4L2_MBUS_PCLK_SAMPLE_RISING);
1099
1100 done:
1101 of_node_put(np);
1102 return pdata;
1103 }
1104
mt9p031_probe(struct i2c_client * client)1105 static int mt9p031_probe(struct i2c_client *client)
1106 {
1107 const struct i2c_device_id *did = i2c_client_get_device_id(client);
1108 struct mt9p031_platform_data *pdata = mt9p031_get_pdata(client);
1109 struct i2c_adapter *adapter = client->adapter;
1110 struct mt9p031 *mt9p031;
1111 unsigned int i;
1112 int ret;
1113
1114 if (pdata == NULL) {
1115 dev_err(&client->dev, "No platform data\n");
1116 return -EINVAL;
1117 }
1118
1119 if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
1120 dev_warn(&client->dev,
1121 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
1122 return -EIO;
1123 }
1124
1125 mt9p031 = devm_kzalloc(&client->dev, sizeof(*mt9p031), GFP_KERNEL);
1126 if (mt9p031 == NULL)
1127 return -ENOMEM;
1128
1129 mt9p031->pdata = pdata;
1130 mt9p031->output_control = MT9P031_OUTPUT_CONTROL_DEF;
1131 mt9p031->mode2 = MT9P031_READ_MODE_2_ROW_BLC;
1132 mt9p031->model = did->driver_data;
1133
1134 mt9p031->regulators[0].supply = "vdd";
1135 mt9p031->regulators[1].supply = "vdd_io";
1136 mt9p031->regulators[2].supply = "vaa";
1137
1138 ret = devm_regulator_bulk_get(&client->dev, 3, mt9p031->regulators);
1139 if (ret < 0) {
1140 dev_err(&client->dev, "Unable to get regulators\n");
1141 return ret;
1142 }
1143
1144 mutex_init(&mt9p031->power_lock);
1145
1146 v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 6);
1147
1148 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1149 V4L2_CID_EXPOSURE, MT9P031_SHUTTER_WIDTH_MIN,
1150 MT9P031_SHUTTER_WIDTH_MAX, 1,
1151 MT9P031_SHUTTER_WIDTH_DEF);
1152 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1153 V4L2_CID_GAIN, MT9P031_GLOBAL_GAIN_MIN,
1154 MT9P031_GLOBAL_GAIN_MAX, 1, MT9P031_GLOBAL_GAIN_DEF);
1155 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1156 V4L2_CID_HFLIP, 0, 1, 1, 0);
1157 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1158 V4L2_CID_VFLIP, 0, 1, 1, 0);
1159 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1160 V4L2_CID_PIXEL_RATE, pdata->target_freq,
1161 pdata->target_freq, 1, pdata->target_freq);
1162 v4l2_ctrl_new_std_menu_items(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1163 V4L2_CID_TEST_PATTERN,
1164 ARRAY_SIZE(mt9p031_test_pattern_menu) - 1, 0,
1165 0, mt9p031_test_pattern_menu);
1166
1167 for (i = 0; i < ARRAY_SIZE(mt9p031_ctrls); ++i)
1168 v4l2_ctrl_new_custom(&mt9p031->ctrls, &mt9p031_ctrls[i], NULL);
1169
1170 mt9p031->subdev.ctrl_handler = &mt9p031->ctrls;
1171
1172 if (mt9p031->ctrls.error) {
1173 printk(KERN_INFO "%s: control initialization error %d\n",
1174 __func__, mt9p031->ctrls.error);
1175 ret = mt9p031->ctrls.error;
1176 goto done;
1177 }
1178
1179 mt9p031->blc_auto = v4l2_ctrl_find(&mt9p031->ctrls, V4L2_CID_BLC_AUTO);
1180 mt9p031->blc_offset = v4l2_ctrl_find(&mt9p031->ctrls,
1181 V4L2_CID_BLC_DIGITAL_OFFSET);
1182
1183 v4l2_i2c_subdev_init(&mt9p031->subdev, client, &mt9p031_subdev_ops);
1184 mt9p031->subdev.internal_ops = &mt9p031_subdev_internal_ops;
1185
1186 mt9p031->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1187 mt9p031->pad.flags = MEDIA_PAD_FL_SOURCE;
1188 ret = media_entity_pads_init(&mt9p031->subdev.entity, 1, &mt9p031->pad);
1189 if (ret < 0)
1190 goto done;
1191
1192 mt9p031->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1193
1194 ret = mt9p031_init_cfg(&mt9p031->subdev, NULL);
1195 if (ret)
1196 goto done;
1197
1198 mt9p031->reset = devm_gpiod_get_optional(&client->dev, "reset",
1199 GPIOD_OUT_HIGH);
1200
1201 ret = mt9p031_clk_setup(mt9p031);
1202 if (ret)
1203 goto done;
1204
1205 ret = v4l2_async_register_subdev(&mt9p031->subdev);
1206
1207 done:
1208 if (ret < 0) {
1209 v4l2_ctrl_handler_free(&mt9p031->ctrls);
1210 media_entity_cleanup(&mt9p031->subdev.entity);
1211 mutex_destroy(&mt9p031->power_lock);
1212 }
1213
1214 return ret;
1215 }
1216
mt9p031_remove(struct i2c_client * client)1217 static void mt9p031_remove(struct i2c_client *client)
1218 {
1219 struct v4l2_subdev *subdev = i2c_get_clientdata(client);
1220 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
1221
1222 v4l2_ctrl_handler_free(&mt9p031->ctrls);
1223 v4l2_async_unregister_subdev(subdev);
1224 media_entity_cleanup(&subdev->entity);
1225 mutex_destroy(&mt9p031->power_lock);
1226 }
1227
1228 static const struct i2c_device_id mt9p031_id[] = {
1229 { "mt9p006", MT9P031_MODEL_COLOR },
1230 { "mt9p031", MT9P031_MODEL_COLOR },
1231 { "mt9p031m", MT9P031_MODEL_MONOCHROME },
1232 { }
1233 };
1234 MODULE_DEVICE_TABLE(i2c, mt9p031_id);
1235
1236 #if IS_ENABLED(CONFIG_OF)
1237 static const struct of_device_id mt9p031_of_match[] = {
1238 { .compatible = "aptina,mt9p006", },
1239 { .compatible = "aptina,mt9p031", },
1240 { .compatible = "aptina,mt9p031m", },
1241 { /* sentinel */ },
1242 };
1243 MODULE_DEVICE_TABLE(of, mt9p031_of_match);
1244 #endif
1245
1246 static struct i2c_driver mt9p031_i2c_driver = {
1247 .driver = {
1248 .of_match_table = of_match_ptr(mt9p031_of_match),
1249 .name = "mt9p031",
1250 },
1251 .probe = mt9p031_probe,
1252 .remove = mt9p031_remove,
1253 .id_table = mt9p031_id,
1254 };
1255
1256 module_i2c_driver(mt9p031_i2c_driver);
1257
1258 MODULE_DESCRIPTION("Aptina MT9P031 Camera driver");
1259 MODULE_AUTHOR("Bastian Hecht <hechtb@gmail.com>");
1260 MODULE_LICENSE("GPL v2");
1261