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