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_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, mt9p031->ext_freq);
241 if (ret < 0)
242 return ret;
243
244 ext_freq = clk_get_rate(mt9p031->clk);
245
246 /* If the external clock frequency is out of bounds for the PLL use the
247 * pixel clock divider only and disable the PLL.
248 */
249 if (ext_freq > limits.ext_clock_max) {
250 unsigned int div;
251
252 div = DIV_ROUND_UP(ext_freq, mt9p031->target_freq);
253 div = roundup_pow_of_two(div) / 2;
254
255 mt9p031->clk_div = min_t(unsigned int, div, 64);
256 mt9p031->use_pll = false;
257
258 return 0;
259 }
260
261 mt9p031->pll.ext_clock = ext_freq;
262 mt9p031->pll.pix_clock = mt9p031->target_freq;
263 mt9p031->use_pll = true;
264
265 return aptina_pll_calculate(&client->dev, &limits, &mt9p031->pll);
266 }
267
mt9p031_pll_enable(struct mt9p031 * mt9p031)268 static int mt9p031_pll_enable(struct mt9p031 *mt9p031)
269 {
270 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
271 int ret;
272
273 if (!mt9p031->use_pll)
274 return 0;
275
276 ret = mt9p031_write(client, MT9P031_PLL_CONTROL,
277 MT9P031_PLL_CONTROL_PWRON);
278 if (ret < 0)
279 return ret;
280
281 ret = mt9p031_write(client, MT9P031_PLL_CONFIG_1,
282 (mt9p031->pll.m << 8) | (mt9p031->pll.n - 1));
283 if (ret < 0)
284 return ret;
285
286 ret = mt9p031_write(client, MT9P031_PLL_CONFIG_2, mt9p031->pll.p1 - 1);
287 if (ret < 0)
288 return ret;
289
290 usleep_range(1000, 2000);
291 ret = mt9p031_write(client, MT9P031_PLL_CONTROL,
292 MT9P031_PLL_CONTROL_PWRON |
293 MT9P031_PLL_CONTROL_USEPLL);
294 return ret;
295 }
296
mt9p031_pll_disable(struct mt9p031 * mt9p031)297 static inline int mt9p031_pll_disable(struct mt9p031 *mt9p031)
298 {
299 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
300
301 if (!mt9p031->use_pll)
302 return 0;
303
304 return mt9p031_write(client, MT9P031_PLL_CONTROL,
305 MT9P031_PLL_CONTROL_PWROFF);
306 }
307
mt9p031_power_on(struct mt9p031 * mt9p031)308 static int mt9p031_power_on(struct mt9p031 *mt9p031)
309 {
310 unsigned long rate, delay;
311 int ret;
312
313 /* Ensure RESET_BAR is active */
314 if (mt9p031->reset) {
315 gpiod_set_value(mt9p031->reset, 1);
316 usleep_range(1000, 2000);
317 }
318
319 /* Bring up the supplies */
320 ret = regulator_bulk_enable(ARRAY_SIZE(mt9p031->regulators),
321 mt9p031->regulators);
322 if (ret < 0)
323 return ret;
324
325 /* Enable clock */
326 if (mt9p031->clk) {
327 ret = clk_prepare_enable(mt9p031->clk);
328 if (ret) {
329 regulator_bulk_disable(ARRAY_SIZE(mt9p031->regulators),
330 mt9p031->regulators);
331 return ret;
332 }
333 }
334
335 /* Now RESET_BAR must be high */
336 if (mt9p031->reset) {
337 gpiod_set_value(mt9p031->reset, 0);
338 /* Wait 850000 EXTCLK cycles before de-asserting reset. */
339 rate = clk_get_rate(mt9p031->clk);
340 if (!rate)
341 rate = 6000000; /* Slowest supported clock, 6 MHz */
342 delay = DIV_ROUND_UP(850000 * 1000, rate);
343 msleep(delay);
344 }
345
346 return 0;
347 }
348
mt9p031_power_off(struct mt9p031 * mt9p031)349 static void mt9p031_power_off(struct mt9p031 *mt9p031)
350 {
351 if (mt9p031->reset) {
352 gpiod_set_value(mt9p031->reset, 1);
353 usleep_range(1000, 2000);
354 }
355
356 regulator_bulk_disable(ARRAY_SIZE(mt9p031->regulators),
357 mt9p031->regulators);
358
359 clk_disable_unprepare(mt9p031->clk);
360 }
361
__mt9p031_set_power(struct mt9p031 * mt9p031,bool on)362 static int __mt9p031_set_power(struct mt9p031 *mt9p031, bool on)
363 {
364 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
365 int ret;
366
367 if (!on) {
368 mt9p031_power_off(mt9p031);
369 return 0;
370 }
371
372 ret = mt9p031_power_on(mt9p031);
373 if (ret < 0)
374 return ret;
375
376 ret = mt9p031_reset(mt9p031);
377 if (ret < 0) {
378 dev_err(&client->dev, "Failed to reset the camera\n");
379 return ret;
380 }
381
382 /* Configure the pixel clock polarity */
383 if (mt9p031->pixclk_pol) {
384 ret = mt9p031_write(client, MT9P031_PIXEL_CLOCK_CONTROL,
385 MT9P031_PIXEL_CLOCK_INVERT);
386 if (ret < 0)
387 return ret;
388 }
389
390 return v4l2_ctrl_handler_setup(&mt9p031->ctrls);
391 }
392
393 /* -----------------------------------------------------------------------------
394 * V4L2 subdev video operations
395 */
396
mt9p031_set_params(struct mt9p031 * mt9p031)397 static int mt9p031_set_params(struct mt9p031 *mt9p031)
398 {
399 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
400 struct v4l2_mbus_framefmt *format = &mt9p031->format;
401 const struct v4l2_rect *crop = &mt9p031->crop;
402 unsigned int hblank;
403 unsigned int vblank;
404 unsigned int xskip;
405 unsigned int yskip;
406 unsigned int xbin;
407 unsigned int ybin;
408 int ret;
409
410 /* Windows position and size.
411 *
412 * TODO: Make sure the start coordinates and window size match the
413 * skipping, binning and mirroring (see description of registers 2 and 4
414 * in table 13, and Binning section on page 41).
415 */
416 ret = mt9p031_write(client, MT9P031_COLUMN_START, crop->left);
417 if (ret < 0)
418 return ret;
419 ret = mt9p031_write(client, MT9P031_ROW_START, crop->top);
420 if (ret < 0)
421 return ret;
422 ret = mt9p031_write(client, MT9P031_WINDOW_WIDTH, crop->width - 1);
423 if (ret < 0)
424 return ret;
425 ret = mt9p031_write(client, MT9P031_WINDOW_HEIGHT, crop->height - 1);
426 if (ret < 0)
427 return ret;
428
429 /* Row and column binning and skipping. Use the maximum binning value
430 * compatible with the skipping settings.
431 */
432 xskip = DIV_ROUND_CLOSEST(crop->width, format->width);
433 yskip = DIV_ROUND_CLOSEST(crop->height, format->height);
434 xbin = 1 << (ffs(xskip) - 1);
435 ybin = 1 << (ffs(yskip) - 1);
436
437 ret = mt9p031_write(client, MT9P031_COLUMN_ADDRESS_MODE,
438 ((xbin - 1) << 4) | (xskip - 1));
439 if (ret < 0)
440 return ret;
441 ret = mt9p031_write(client, MT9P031_ROW_ADDRESS_MODE,
442 ((ybin - 1) << 4) | (yskip - 1));
443 if (ret < 0)
444 return ret;
445
446 /* Blanking - use minimum value for horizontal blanking and default
447 * value for vertical blanking.
448 */
449 hblank = 346 * ybin + 64 + (80 >> min_t(unsigned int, xbin, 3));
450 vblank = MT9P031_VERTICAL_BLANK_DEF;
451
452 ret = mt9p031_write(client, MT9P031_HORIZONTAL_BLANK, hblank - 1);
453 if (ret < 0)
454 return ret;
455 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_state_get_format(sd_state, pad);
553 case V4L2_SUBDEV_FORMAT_ACTIVE:
554 return &mt9p031->format;
555 default:
556 return NULL;
557 }
558 }
559
560 static struct v4l2_rect *
__mt9p031_get_pad_crop(struct mt9p031 * mt9p031,struct v4l2_subdev_state * sd_state,unsigned int pad,u32 which)561 __mt9p031_get_pad_crop(struct mt9p031 *mt9p031,
562 struct v4l2_subdev_state *sd_state,
563 unsigned int pad, u32 which)
564 {
565 switch (which) {
566 case V4L2_SUBDEV_FORMAT_TRY:
567 return v4l2_subdev_state_get_crop(sd_state, pad);
568 case V4L2_SUBDEV_FORMAT_ACTIVE:
569 return &mt9p031->crop;
570 default:
571 return NULL;
572 }
573 }
574
mt9p031_get_format(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * fmt)575 static int mt9p031_get_format(struct v4l2_subdev *subdev,
576 struct v4l2_subdev_state *sd_state,
577 struct v4l2_subdev_format *fmt)
578 {
579 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
580
581 fmt->format = *__mt9p031_get_pad_format(mt9p031, sd_state, fmt->pad,
582 fmt->which);
583 return 0;
584 }
585
mt9p031_set_format(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * format)586 static int mt9p031_set_format(struct v4l2_subdev *subdev,
587 struct v4l2_subdev_state *sd_state,
588 struct v4l2_subdev_format *format)
589 {
590 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
591 struct v4l2_mbus_framefmt *__format;
592 struct v4l2_rect *__crop;
593 unsigned int width;
594 unsigned int height;
595 unsigned int hratio;
596 unsigned int vratio;
597
598 __crop = __mt9p031_get_pad_crop(mt9p031, sd_state, format->pad,
599 format->which);
600
601 /* Clamp the width and height to avoid dividing by zero. */
602 width = clamp_t(unsigned int, ALIGN(format->format.width, 2),
603 max_t(unsigned int, __crop->width / 7,
604 MT9P031_WINDOW_WIDTH_MIN),
605 __crop->width);
606 height = clamp_t(unsigned int, ALIGN(format->format.height, 2),
607 max_t(unsigned int, __crop->height / 8,
608 MT9P031_WINDOW_HEIGHT_MIN),
609 __crop->height);
610
611 hratio = DIV_ROUND_CLOSEST(__crop->width, width);
612 vratio = DIV_ROUND_CLOSEST(__crop->height, height);
613
614 __format = __mt9p031_get_pad_format(mt9p031, sd_state, format->pad,
615 format->which);
616 __format->width = __crop->width / hratio;
617 __format->height = __crop->height / vratio;
618
619 format->format = *__format;
620
621 return 0;
622 }
623
mt9p031_get_selection(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_selection * sel)624 static int mt9p031_get_selection(struct v4l2_subdev *subdev,
625 struct v4l2_subdev_state *sd_state,
626 struct v4l2_subdev_selection *sel)
627 {
628 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
629
630 switch (sel->target) {
631 case V4L2_SEL_TGT_CROP_BOUNDS:
632 sel->r.left = MT9P031_COLUMN_START_MIN;
633 sel->r.top = MT9P031_ROW_START_MIN;
634 sel->r.width = MT9P031_WINDOW_WIDTH_MAX;
635 sel->r.height = MT9P031_WINDOW_HEIGHT_MAX;
636 return 0;
637
638 case V4L2_SEL_TGT_CROP:
639 sel->r = *__mt9p031_get_pad_crop(mt9p031, sd_state,
640 sel->pad, sel->which);
641 return 0;
642
643 default:
644 return -EINVAL;
645 }
646 }
647
mt9p031_set_selection(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_selection * sel)648 static int mt9p031_set_selection(struct v4l2_subdev *subdev,
649 struct v4l2_subdev_state *sd_state,
650 struct v4l2_subdev_selection *sel)
651 {
652 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
653 struct v4l2_mbus_framefmt *__format;
654 struct v4l2_rect *__crop;
655 struct v4l2_rect rect;
656
657 if (sel->target != V4L2_SEL_TGT_CROP)
658 return -EINVAL;
659
660 /* Clamp the crop rectangle boundaries and align them to a multiple of 2
661 * pixels to ensure a GRBG Bayer pattern.
662 */
663 rect.left = clamp(ALIGN(sel->r.left, 2), MT9P031_COLUMN_START_MIN,
664 MT9P031_COLUMN_START_MAX);
665 rect.top = clamp(ALIGN(sel->r.top, 2), MT9P031_ROW_START_MIN,
666 MT9P031_ROW_START_MAX);
667 rect.width = clamp_t(unsigned int, ALIGN(sel->r.width, 2),
668 MT9P031_WINDOW_WIDTH_MIN,
669 MT9P031_WINDOW_WIDTH_MAX);
670 rect.height = clamp_t(unsigned int, ALIGN(sel->r.height, 2),
671 MT9P031_WINDOW_HEIGHT_MIN,
672 MT9P031_WINDOW_HEIGHT_MAX);
673
674 rect.width = min_t(unsigned int, rect.width,
675 MT9P031_PIXEL_ARRAY_WIDTH - rect.left);
676 rect.height = min_t(unsigned int, rect.height,
677 MT9P031_PIXEL_ARRAY_HEIGHT - rect.top);
678
679 __crop = __mt9p031_get_pad_crop(mt9p031, sd_state, sel->pad,
680 sel->which);
681
682 if (rect.width != __crop->width || rect.height != __crop->height) {
683 /* Reset the output image size if the crop rectangle size has
684 * been modified.
685 */
686 __format = __mt9p031_get_pad_format(mt9p031, sd_state,
687 sel->pad,
688 sel->which);
689 __format->width = rect.width;
690 __format->height = rect.height;
691 }
692
693 *__crop = rect;
694 sel->r = rect;
695
696 return 0;
697 }
698
mt9p031_init_state(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state)699 static int mt9p031_init_state(struct v4l2_subdev *subdev,
700 struct v4l2_subdev_state *sd_state)
701 {
702 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
703 struct v4l2_mbus_framefmt *format;
704 struct v4l2_rect *crop;
705 const int which = sd_state == NULL ? V4L2_SUBDEV_FORMAT_ACTIVE :
706 V4L2_SUBDEV_FORMAT_TRY;
707
708 crop = __mt9p031_get_pad_crop(mt9p031, sd_state, 0, which);
709 crop->left = MT9P031_COLUMN_START_DEF;
710 crop->top = MT9P031_ROW_START_DEF;
711 crop->width = MT9P031_WINDOW_WIDTH_DEF;
712 crop->height = MT9P031_WINDOW_HEIGHT_DEF;
713
714 format = __mt9p031_get_pad_format(mt9p031, sd_state, 0, which);
715 format->code = mt9p031->code;
716 format->width = MT9P031_WINDOW_WIDTH_DEF;
717 format->height = MT9P031_WINDOW_HEIGHT_DEF;
718 format->field = V4L2_FIELD_NONE;
719 format->colorspace = V4L2_COLORSPACE_SRGB;
720
721 return 0;
722 }
723
724 /* -----------------------------------------------------------------------------
725 * V4L2 subdev control operations
726 */
727
728 #define V4L2_CID_BLC_AUTO (V4L2_CID_USER_BASE | 0x1002)
729 #define V4L2_CID_BLC_TARGET_LEVEL (V4L2_CID_USER_BASE | 0x1003)
730 #define V4L2_CID_BLC_ANALOG_OFFSET (V4L2_CID_USER_BASE | 0x1004)
731 #define V4L2_CID_BLC_DIGITAL_OFFSET (V4L2_CID_USER_BASE | 0x1005)
732
mt9p031_restore_blc(struct mt9p031 * mt9p031)733 static int mt9p031_restore_blc(struct mt9p031 *mt9p031)
734 {
735 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
736 int ret;
737
738 if (mt9p031->blc_auto->cur.val != 0) {
739 ret = mt9p031_set_mode2(mt9p031, 0,
740 MT9P031_READ_MODE_2_ROW_BLC);
741 if (ret < 0)
742 return ret;
743 }
744
745 if (mt9p031->blc_offset->cur.val != 0) {
746 ret = mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
747 mt9p031->blc_offset->cur.val);
748 if (ret < 0)
749 return ret;
750 }
751
752 return 0;
753 }
754
mt9p031_s_ctrl(struct v4l2_ctrl * ctrl)755 static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
756 {
757 struct mt9p031 *mt9p031 =
758 container_of(ctrl->handler, struct mt9p031, ctrls);
759 struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
760 u16 data;
761 int ret;
762
763 if (ctrl->flags & V4L2_CTRL_FLAG_INACTIVE)
764 return 0;
765
766 switch (ctrl->id) {
767 case V4L2_CID_EXPOSURE:
768 ret = mt9p031_write(client, MT9P031_SHUTTER_WIDTH_UPPER,
769 (ctrl->val >> 16) & 0xffff);
770 if (ret < 0)
771 return ret;
772
773 return mt9p031_write(client, MT9P031_SHUTTER_WIDTH_LOWER,
774 ctrl->val & 0xffff);
775
776 case V4L2_CID_GAIN:
777 /* Gain is controlled by 2 analog stages and a digital stage.
778 * Valid values for the 3 stages are
779 *
780 * Stage Min Max Step
781 * ------------------------------------------
782 * First analog stage x1 x2 1
783 * Second analog stage x1 x4 0.125
784 * Digital stage x1 x16 0.125
785 *
786 * To minimize noise, the gain stages should be used in the
787 * second analog stage, first analog stage, digital stage order.
788 * Gain from a previous stage should be pushed to its maximum
789 * value before the next stage is used.
790 */
791 if (ctrl->val <= 32) {
792 data = ctrl->val;
793 } else if (ctrl->val <= 64) {
794 ctrl->val &= ~1;
795 data = (1 << 6) | (ctrl->val >> 1);
796 } else {
797 ctrl->val &= ~7;
798 data = ((ctrl->val - 64) << 5) | (1 << 6) | 32;
799 }
800
801 return mt9p031_write(client, MT9P031_GLOBAL_GAIN, data);
802
803 case V4L2_CID_HFLIP:
804 if (ctrl->val)
805 return mt9p031_set_mode2(mt9p031,
806 0, MT9P031_READ_MODE_2_COL_MIR);
807 else
808 return mt9p031_set_mode2(mt9p031,
809 MT9P031_READ_MODE_2_COL_MIR, 0);
810
811 case V4L2_CID_VFLIP:
812 if (ctrl->val)
813 return mt9p031_set_mode2(mt9p031,
814 0, MT9P031_READ_MODE_2_ROW_MIR);
815 else
816 return mt9p031_set_mode2(mt9p031,
817 MT9P031_READ_MODE_2_ROW_MIR, 0);
818
819 case V4L2_CID_TEST_PATTERN:
820 /* The digital side of the Black Level Calibration function must
821 * be disabled when generating a test pattern to avoid artifacts
822 * in the image. Activate (deactivate) the BLC-related controls
823 * when the test pattern is enabled (disabled).
824 */
825 v4l2_ctrl_activate(mt9p031->blc_auto, ctrl->val == 0);
826 v4l2_ctrl_activate(mt9p031->blc_offset, ctrl->val == 0);
827
828 if (!ctrl->val) {
829 /* Restore the BLC settings. */
830 ret = mt9p031_restore_blc(mt9p031);
831 if (ret < 0)
832 return ret;
833
834 return mt9p031_write(client, MT9P031_TEST_PATTERN, 0);
835 }
836
837 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_GREEN, 0x05a0);
838 if (ret < 0)
839 return ret;
840 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_RED, 0x0a50);
841 if (ret < 0)
842 return ret;
843 ret = mt9p031_write(client, MT9P031_TEST_PATTERN_BLUE, 0x0aa0);
844 if (ret < 0)
845 return ret;
846
847 /* Disable digital BLC when generating a test pattern. */
848 ret = mt9p031_set_mode2(mt9p031, MT9P031_READ_MODE_2_ROW_BLC,
849 0);
850 if (ret < 0)
851 return ret;
852
853 ret = mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET, 0);
854 if (ret < 0)
855 return ret;
856
857 return mt9p031_write(client, MT9P031_TEST_PATTERN,
858 ((ctrl->val - 1) << MT9P031_TEST_PATTERN_SHIFT)
859 | MT9P031_TEST_PATTERN_ENABLE);
860
861 case V4L2_CID_BLC_AUTO:
862 ret = mt9p031_set_mode2(mt9p031,
863 ctrl->val ? 0 : MT9P031_READ_MODE_2_ROW_BLC,
864 ctrl->val ? MT9P031_READ_MODE_2_ROW_BLC : 0);
865 if (ret < 0)
866 return ret;
867
868 return mt9p031_write(client, MT9P031_BLACK_LEVEL_CALIBRATION,
869 ctrl->val ? 0 : MT9P031_BLC_MANUAL_BLC);
870
871 case V4L2_CID_BLC_TARGET_LEVEL:
872 return mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
873 ctrl->val);
874
875 case V4L2_CID_BLC_ANALOG_OFFSET:
876 data = ctrl->val & ((1 << 9) - 1);
877
878 ret = mt9p031_write(client, MT9P031_GREEN1_OFFSET, data);
879 if (ret < 0)
880 return ret;
881 ret = mt9p031_write(client, MT9P031_GREEN2_OFFSET, data);
882 if (ret < 0)
883 return ret;
884 ret = mt9p031_write(client, MT9P031_RED_OFFSET, data);
885 if (ret < 0)
886 return ret;
887 return mt9p031_write(client, MT9P031_BLUE_OFFSET, data);
888
889 case V4L2_CID_BLC_DIGITAL_OFFSET:
890 return mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET,
891 ctrl->val & ((1 << 12) - 1));
892 }
893
894 return 0;
895 }
896
897 static const struct v4l2_ctrl_ops mt9p031_ctrl_ops = {
898 .s_ctrl = mt9p031_s_ctrl,
899 };
900
901 static const char * const mt9p031_test_pattern_menu[] = {
902 "Disabled",
903 "Color Field",
904 "Horizontal Gradient",
905 "Vertical Gradient",
906 "Diagonal Gradient",
907 "Classic Test Pattern",
908 "Walking 1s",
909 "Monochrome Horizontal Bars",
910 "Monochrome Vertical Bars",
911 "Vertical Color Bars",
912 };
913
914 static const struct v4l2_ctrl_config mt9p031_ctrls[] = {
915 {
916 .ops = &mt9p031_ctrl_ops,
917 .id = V4L2_CID_BLC_AUTO,
918 .type = V4L2_CTRL_TYPE_BOOLEAN,
919 .name = "BLC, Auto",
920 .min = 0,
921 .max = 1,
922 .step = 1,
923 .def = 1,
924 .flags = 0,
925 }, {
926 .ops = &mt9p031_ctrl_ops,
927 .id = V4L2_CID_BLC_TARGET_LEVEL,
928 .type = V4L2_CTRL_TYPE_INTEGER,
929 .name = "BLC Target Level",
930 .min = 0,
931 .max = 4095,
932 .step = 1,
933 .def = 168,
934 .flags = 0,
935 }, {
936 .ops = &mt9p031_ctrl_ops,
937 .id = V4L2_CID_BLC_ANALOG_OFFSET,
938 .type = V4L2_CTRL_TYPE_INTEGER,
939 .name = "BLC Analog Offset",
940 .min = -255,
941 .max = 255,
942 .step = 1,
943 .def = 32,
944 .flags = 0,
945 }, {
946 .ops = &mt9p031_ctrl_ops,
947 .id = V4L2_CID_BLC_DIGITAL_OFFSET,
948 .type = V4L2_CTRL_TYPE_INTEGER,
949 .name = "BLC Digital Offset",
950 .min = -2048,
951 .max = 2047,
952 .step = 1,
953 .def = 40,
954 .flags = 0,
955 }
956 };
957
958 /* -----------------------------------------------------------------------------
959 * V4L2 subdev core operations
960 */
961
mt9p031_set_power(struct v4l2_subdev * subdev,int on)962 static int mt9p031_set_power(struct v4l2_subdev *subdev, int on)
963 {
964 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
965 int ret = 0;
966
967 mutex_lock(&mt9p031->power_lock);
968
969 /* If the power count is modified from 0 to != 0 or from != 0 to 0,
970 * update the power state.
971 */
972 if (mt9p031->power_count == !on) {
973 ret = __mt9p031_set_power(mt9p031, !!on);
974 if (ret < 0)
975 goto out;
976 }
977
978 /* Update the power count. */
979 mt9p031->power_count += on ? 1 : -1;
980 WARN_ON(mt9p031->power_count < 0);
981
982 out:
983 mutex_unlock(&mt9p031->power_lock);
984 return ret;
985 }
986
987 /* -----------------------------------------------------------------------------
988 * V4L2 subdev internal operations
989 */
990
mt9p031_registered(struct v4l2_subdev * subdev)991 static int mt9p031_registered(struct v4l2_subdev *subdev)
992 {
993 struct i2c_client *client = v4l2_get_subdevdata(subdev);
994 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
995 s32 data;
996 int ret;
997
998 ret = mt9p031_power_on(mt9p031);
999 if (ret < 0) {
1000 dev_err(&client->dev, "MT9P031 power up failed\n");
1001 return ret;
1002 }
1003
1004 /* Read out the chip version register */
1005 data = mt9p031_read(client, MT9P031_CHIP_VERSION);
1006 mt9p031_power_off(mt9p031);
1007
1008 if (data != MT9P031_CHIP_VERSION_VALUE) {
1009 dev_err(&client->dev, "MT9P031 not detected, wrong version "
1010 "0x%04x\n", data);
1011 return -ENODEV;
1012 }
1013
1014 dev_info(&client->dev, "MT9P031 detected at address 0x%02x\n",
1015 client->addr);
1016
1017 return 0;
1018 }
1019
mt9p031_open(struct v4l2_subdev * subdev,struct v4l2_subdev_fh * fh)1020 static int mt9p031_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
1021 {
1022 return mt9p031_set_power(subdev, 1);
1023 }
1024
mt9p031_close(struct v4l2_subdev * subdev,struct v4l2_subdev_fh * fh)1025 static int mt9p031_close(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
1026 {
1027 return mt9p031_set_power(subdev, 0);
1028 }
1029
1030 static const struct v4l2_subdev_core_ops mt9p031_subdev_core_ops = {
1031 .s_power = mt9p031_set_power,
1032 };
1033
1034 static const struct v4l2_subdev_video_ops mt9p031_subdev_video_ops = {
1035 .s_stream = mt9p031_s_stream,
1036 };
1037
1038 static const struct v4l2_subdev_pad_ops mt9p031_subdev_pad_ops = {
1039 .enum_mbus_code = mt9p031_enum_mbus_code,
1040 .enum_frame_size = mt9p031_enum_frame_size,
1041 .get_fmt = mt9p031_get_format,
1042 .set_fmt = mt9p031_set_format,
1043 .get_selection = mt9p031_get_selection,
1044 .set_selection = mt9p031_set_selection,
1045 };
1046
1047 static const struct v4l2_subdev_ops mt9p031_subdev_ops = {
1048 .core = &mt9p031_subdev_core_ops,
1049 .video = &mt9p031_subdev_video_ops,
1050 .pad = &mt9p031_subdev_pad_ops,
1051 };
1052
1053 static const struct v4l2_subdev_internal_ops mt9p031_subdev_internal_ops = {
1054 .init_state = mt9p031_init_state,
1055 .registered = mt9p031_registered,
1056 .open = mt9p031_open,
1057 .close = mt9p031_close,
1058 };
1059
1060 /* -----------------------------------------------------------------------------
1061 * Driver initialization and probing
1062 */
1063
mt9p031_parse_properties(struct mt9p031 * mt9p031,struct device * dev)1064 static int mt9p031_parse_properties(struct mt9p031 *mt9p031, struct device *dev)
1065 {
1066 struct v4l2_fwnode_endpoint endpoint = {
1067 .bus_type = V4L2_MBUS_PARALLEL
1068 };
1069 struct fwnode_handle *np;
1070 int ret;
1071
1072 np = fwnode_graph_get_next_endpoint(dev_fwnode(dev), NULL);
1073 if (!np)
1074 return dev_err_probe(dev, -EINVAL, "endpoint node not found\n");
1075
1076 ret = v4l2_fwnode_endpoint_parse(np, &endpoint);
1077 fwnode_handle_put(np);
1078 if (ret)
1079 return dev_err_probe(dev, -EINVAL, "could not parse endpoint\n");
1080
1081 fwnode_property_read_u32(np, "input-clock-frequency",
1082 &mt9p031->ext_freq);
1083 fwnode_property_read_u32(np, "pixel-clock-frequency",
1084 &mt9p031->target_freq);
1085
1086 mt9p031->pixclk_pol = !!(endpoint.bus.parallel.flags &
1087 V4L2_MBUS_PCLK_SAMPLE_RISING);
1088
1089 return 0;
1090 }
1091
mt9p031_probe(struct i2c_client * client)1092 static int mt9p031_probe(struct i2c_client *client)
1093 {
1094 struct i2c_adapter *adapter = client->adapter;
1095 struct mt9p031 *mt9p031;
1096 unsigned int i;
1097 int ret;
1098
1099 if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
1100 dev_warn(&client->dev,
1101 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
1102 return -EIO;
1103 }
1104
1105 mt9p031 = devm_kzalloc(&client->dev, sizeof(*mt9p031), GFP_KERNEL);
1106 if (mt9p031 == NULL)
1107 return -ENOMEM;
1108
1109 ret = mt9p031_parse_properties(mt9p031, &client->dev);
1110 if (ret)
1111 return ret;
1112
1113 mt9p031->output_control = MT9P031_OUTPUT_CONTROL_DEF;
1114 mt9p031->mode2 = MT9P031_READ_MODE_2_ROW_BLC;
1115 mt9p031->code = (uintptr_t)device_get_match_data(&client->dev);
1116
1117 mt9p031->regulators[0].supply = "vdd";
1118 mt9p031->regulators[1].supply = "vdd_io";
1119 mt9p031->regulators[2].supply = "vaa";
1120
1121 ret = devm_regulator_bulk_get(&client->dev, 3, mt9p031->regulators);
1122 if (ret < 0) {
1123 dev_err(&client->dev, "Unable to get regulators\n");
1124 return ret;
1125 }
1126
1127 mutex_init(&mt9p031->power_lock);
1128
1129 v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 6);
1130
1131 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1132 V4L2_CID_EXPOSURE, MT9P031_SHUTTER_WIDTH_MIN,
1133 MT9P031_SHUTTER_WIDTH_MAX, 1,
1134 MT9P031_SHUTTER_WIDTH_DEF);
1135 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1136 V4L2_CID_GAIN, MT9P031_GLOBAL_GAIN_MIN,
1137 MT9P031_GLOBAL_GAIN_MAX, 1, MT9P031_GLOBAL_GAIN_DEF);
1138 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1139 V4L2_CID_HFLIP, 0, 1, 1, 0);
1140 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1141 V4L2_CID_VFLIP, 0, 1, 1, 0);
1142 v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1143 V4L2_CID_PIXEL_RATE, mt9p031->target_freq,
1144 mt9p031->target_freq, 1, mt9p031->target_freq);
1145 v4l2_ctrl_new_std_menu_items(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1146 V4L2_CID_TEST_PATTERN,
1147 ARRAY_SIZE(mt9p031_test_pattern_menu) - 1, 0,
1148 0, mt9p031_test_pattern_menu);
1149
1150 for (i = 0; i < ARRAY_SIZE(mt9p031_ctrls); ++i)
1151 v4l2_ctrl_new_custom(&mt9p031->ctrls, &mt9p031_ctrls[i], NULL);
1152
1153 mt9p031->subdev.ctrl_handler = &mt9p031->ctrls;
1154
1155 if (mt9p031->ctrls.error) {
1156 printk(KERN_INFO "%s: control initialization error %d\n",
1157 __func__, mt9p031->ctrls.error);
1158 ret = mt9p031->ctrls.error;
1159 goto done;
1160 }
1161
1162 mt9p031->blc_auto = v4l2_ctrl_find(&mt9p031->ctrls, V4L2_CID_BLC_AUTO);
1163 mt9p031->blc_offset = v4l2_ctrl_find(&mt9p031->ctrls,
1164 V4L2_CID_BLC_DIGITAL_OFFSET);
1165
1166 v4l2_i2c_subdev_init(&mt9p031->subdev, client, &mt9p031_subdev_ops);
1167 mt9p031->subdev.internal_ops = &mt9p031_subdev_internal_ops;
1168
1169 mt9p031->subdev.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1170 mt9p031->pad.flags = MEDIA_PAD_FL_SOURCE;
1171 ret = media_entity_pads_init(&mt9p031->subdev.entity, 1, &mt9p031->pad);
1172 if (ret < 0)
1173 goto done;
1174
1175 mt9p031->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1176
1177 ret = mt9p031_init_state(&mt9p031->subdev, NULL);
1178 if (ret)
1179 goto done;
1180
1181 mt9p031->reset = devm_gpiod_get_optional(&client->dev, "reset",
1182 GPIOD_OUT_HIGH);
1183
1184 ret = mt9p031_clk_setup(mt9p031);
1185 if (ret)
1186 goto done;
1187
1188 ret = v4l2_async_register_subdev(&mt9p031->subdev);
1189
1190 done:
1191 if (ret < 0) {
1192 v4l2_ctrl_handler_free(&mt9p031->ctrls);
1193 media_entity_cleanup(&mt9p031->subdev.entity);
1194 mutex_destroy(&mt9p031->power_lock);
1195 }
1196
1197 return ret;
1198 }
1199
mt9p031_remove(struct i2c_client * client)1200 static void mt9p031_remove(struct i2c_client *client)
1201 {
1202 struct v4l2_subdev *subdev = i2c_get_clientdata(client);
1203 struct mt9p031 *mt9p031 = to_mt9p031(subdev);
1204
1205 v4l2_ctrl_handler_free(&mt9p031->ctrls);
1206 v4l2_async_unregister_subdev(subdev);
1207 media_entity_cleanup(&subdev->entity);
1208 mutex_destroy(&mt9p031->power_lock);
1209 }
1210
1211 static const struct mt9p031_model_info mt9p031_models_bayer = {
1212 .code = MEDIA_BUS_FMT_SGRBG12_1X12
1213 };
1214
1215 static const struct mt9p031_model_info mt9p031_models_mono = {
1216 .code = MEDIA_BUS_FMT_Y12_1X12
1217 };
1218
1219 static const struct of_device_id mt9p031_of_match[] = {
1220 { .compatible = "aptina,mt9p006", .data = &mt9p031_models_bayer },
1221 { .compatible = "aptina,mt9p031", .data = &mt9p031_models_bayer },
1222 { .compatible = "aptina,mt9p031m", .data = &mt9p031_models_mono },
1223 { /* sentinel */ }
1224 };
1225 MODULE_DEVICE_TABLE(of, mt9p031_of_match);
1226
1227 static struct i2c_driver mt9p031_i2c_driver = {
1228 .driver = {
1229 .of_match_table = mt9p031_of_match,
1230 .name = "mt9p031",
1231 },
1232 .probe = mt9p031_probe,
1233 .remove = mt9p031_remove,
1234 };
1235
1236 module_i2c_driver(mt9p031_i2c_driver);
1237
1238 MODULE_DESCRIPTION("Aptina MT9P031 Camera driver");
1239 MODULE_AUTHOR("Bastian Hecht <hechtb@gmail.com>");
1240 MODULE_LICENSE("GPL v2");
1241