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