1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * V4L2 sensor driver for Aptina MT9V111 image sensor
4 * Copyright (C) 2018 Jacopo Mondi <jacopo@jmondi.org>
5 *
6 * Based on mt9v032 driver
7 * Copyright (C) 2010, Laurent Pinchart <laurent.pinchart@ideasonboard.com>
8 * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>
9 *
10 * Based on mt9v011 driver
11 * Copyright (c) 2009 Mauro Carvalho Chehab <mchehab@kernel.org>
12 */
13
14 #include <linux/clk.h>
15 #include <linux/delay.h>
16 #include <linux/gpio/consumer.h>
17 #include <linux/i2c.h>
18 #include <linux/of.h>
19 #include <linux/slab.h>
20 #include <linux/videodev2.h>
21 #include <linux/v4l2-mediabus.h>
22 #include <linux/module.h>
23
24 #include <media/v4l2-ctrls.h>
25 #include <media/v4l2-device.h>
26 #include <media/v4l2-fwnode.h>
27 #include <media/v4l2-image-sizes.h>
28 #include <media/v4l2-subdev.h>
29
30 /*
31 * MT9V111 is a 1/4-Inch CMOS digital image sensor with an integrated
32 * Image Flow Processing (IFP) engine and a sensor core loosely based on
33 * MT9V011.
34 *
35 * The IFP can produce several output image formats from the sensor core
36 * output. This driver currently supports only YUYV format permutations.
37 *
38 * The driver allows manual frame rate control through s_frame_interval subdev
39 * operation or V4L2_CID_V/HBLANK controls, but it is known that the
40 * auto-exposure algorithm might modify the programmed frame rate. While the
41 * driver initially programs the sensor with auto-exposure and
42 * auto-white-balancing enabled, it is possible to disable them and more
43 * precisely control the frame rate.
44 *
45 * While it seems possible to instruct the auto-exposure control algorithm to
46 * respect a programmed frame rate when adjusting the pixel integration time,
47 * registers controlling this feature are not documented in the public
48 * available sensor manual used to develop this driver (09005aef80e90084,
49 * MT9V111_1.fm - Rev. G 1/05 EN).
50 */
51
52 #define MT9V111_CHIP_ID_HIGH 0x82
53 #define MT9V111_CHIP_ID_LOW 0x3a
54
55 #define MT9V111_R01_ADDR_SPACE 0x01
56 #define MT9V111_R01_IFP 0x01
57 #define MT9V111_R01_CORE 0x04
58
59 #define MT9V111_IFP_R06_OPMODE_CTRL 0x06
60 #define MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN BIT(1)
61 #define MT9V111_IFP_R06_OPMODE_CTRL_AE_EN BIT(14)
62 #define MT9V111_IFP_R07_IFP_RESET 0x07
63 #define MT9V111_IFP_R07_IFP_RESET_MASK BIT(0)
64 #define MT9V111_IFP_R08_OUTFMT_CTRL 0x08
65 #define MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER BIT(11)
66 #define MT9V111_IFP_R08_OUTFMT_CTRL_PCLK BIT(5)
67 #define MT9V111_IFP_R3A_OUTFMT_CTRL2 0x3a
68 #define MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_CBCR BIT(0)
69 #define MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_YC BIT(1)
70 #define MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_MASK GENMASK(2, 0)
71 #define MT9V111_IFP_RA5_HPAN 0xa5
72 #define MT9V111_IFP_RA6_HZOOM 0xa6
73 #define MT9V111_IFP_RA7_HOUT 0xa7
74 #define MT9V111_IFP_RA8_VPAN 0xa8
75 #define MT9V111_IFP_RA9_VZOOM 0xa9
76 #define MT9V111_IFP_RAA_VOUT 0xaa
77 #define MT9V111_IFP_DECIMATION_MASK GENMASK(9, 0)
78 #define MT9V111_IFP_DECIMATION_FREEZE BIT(15)
79
80 #define MT9V111_CORE_R03_WIN_HEIGHT 0x03
81 #define MT9V111_CORE_R03_WIN_V_OFFS 2
82 #define MT9V111_CORE_R04_WIN_WIDTH 0x04
83 #define MT9V111_CORE_R04_WIN_H_OFFS 114
84 #define MT9V111_CORE_R05_HBLANK 0x05
85 #define MT9V111_CORE_R05_MIN_HBLANK 0x09
86 #define MT9V111_CORE_R05_MAX_HBLANK GENMASK(9, 0)
87 #define MT9V111_CORE_R05_DEF_HBLANK 0x26
88 #define MT9V111_CORE_R06_VBLANK 0x06
89 #define MT9V111_CORE_R06_MIN_VBLANK 0x03
90 #define MT9V111_CORE_R06_MAX_VBLANK GENMASK(11, 0)
91 #define MT9V111_CORE_R06_DEF_VBLANK 0x04
92 #define MT9V111_CORE_R07_OUT_CTRL 0x07
93 #define MT9V111_CORE_R07_OUT_CTRL_SAMPLE BIT(4)
94 #define MT9V111_CORE_R09_PIXEL_INT 0x09
95 #define MT9V111_CORE_R09_PIXEL_INT_MASK GENMASK(11, 0)
96 #define MT9V111_CORE_R0D_CORE_RESET 0x0d
97 #define MT9V111_CORE_R0D_CORE_RESET_MASK BIT(0)
98 #define MT9V111_CORE_RFF_CHIP_VER 0xff
99
100 #define MT9V111_PIXEL_ARRAY_WIDTH 640
101 #define MT9V111_PIXEL_ARRAY_HEIGHT 480
102
103 #define MT9V111_MAX_CLKIN 27000000
104
105 /* The default sensor configuration at startup time. */
106 static const struct v4l2_mbus_framefmt mt9v111_def_fmt = {
107 .width = 640,
108 .height = 480,
109 .code = MEDIA_BUS_FMT_UYVY8_2X8,
110 .field = V4L2_FIELD_NONE,
111 .colorspace = V4L2_COLORSPACE_SRGB,
112 .ycbcr_enc = V4L2_YCBCR_ENC_601,
113 .quantization = V4L2_QUANTIZATION_LIM_RANGE,
114 .xfer_func = V4L2_XFER_FUNC_SRGB,
115 };
116
117 struct mt9v111_dev {
118 struct device *dev;
119 struct i2c_client *client;
120
121 u8 addr_space;
122
123 struct v4l2_subdev sd;
124 #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
125 struct media_pad pad;
126 #endif
127
128 struct v4l2_ctrl *auto_awb;
129 struct v4l2_ctrl *auto_exp;
130 struct v4l2_ctrl *hblank;
131 struct v4l2_ctrl *vblank;
132 struct v4l2_ctrl_handler ctrls;
133
134 /* Output image format and sizes. */
135 struct v4l2_mbus_framefmt fmt;
136 unsigned int fps;
137
138 /* Protects power up/down sequences. */
139 struct mutex pwr_mutex;
140 int pwr_count;
141
142 /* Protects stream on/off sequences. */
143 struct mutex stream_mutex;
144 bool streaming;
145
146 /* Flags to mark HW settings as not yet applied. */
147 bool pending;
148
149 /* Clock provider and system clock frequency. */
150 struct clk *clk;
151 u32 sysclk;
152
153 struct gpio_desc *oe;
154 struct gpio_desc *standby;
155 struct gpio_desc *reset;
156 };
157
158 #define sd_to_mt9v111(__sd) container_of((__sd), struct mt9v111_dev, sd)
159
160 /*
161 * mt9v111_mbus_fmt - List all media bus formats supported by the driver.
162 *
163 * Only list the media bus code here. The image sizes are freely configurable
164 * in the pixel array sizes range.
165 *
166 * The desired frame interval, in the supported frame interval range, is
167 * obtained by configuring blanking as the sensor does not have a PLL but
168 * only a fixed clock divider that generates the output pixel clock.
169 */
170 static struct mt9v111_mbus_fmt {
171 u32 code;
172 } mt9v111_formats[] = {
173 {
174 .code = MEDIA_BUS_FMT_UYVY8_2X8,
175 },
176 {
177 .code = MEDIA_BUS_FMT_YUYV8_2X8,
178 },
179 {
180 .code = MEDIA_BUS_FMT_VYUY8_2X8,
181 },
182 {
183 .code = MEDIA_BUS_FMT_YVYU8_2X8,
184 },
185 };
186
187 static u32 mt9v111_frame_intervals[] = {5, 10, 15, 20, 30};
188
189 /*
190 * mt9v111_frame_sizes - List sensor's supported resolutions.
191 *
192 * Resolution generated through decimation in the IFP block from the
193 * full VGA pixel array.
194 */
195 static struct v4l2_rect mt9v111_frame_sizes[] = {
196 {
197 .width = 640,
198 .height = 480,
199 },
200 {
201 .width = 352,
202 .height = 288
203 },
204 {
205 .width = 320,
206 .height = 240,
207 },
208 {
209 .width = 176,
210 .height = 144,
211 },
212 {
213 .width = 160,
214 .height = 120,
215 },
216 };
217
218 /* --- Device I/O access --- */
219
__mt9v111_read(struct i2c_client * c,u8 reg,u16 * val)220 static int __mt9v111_read(struct i2c_client *c, u8 reg, u16 *val)
221 {
222 struct i2c_msg msg[2];
223 __be16 buf;
224 int ret;
225
226 msg[0].addr = c->addr;
227 msg[0].flags = 0;
228 msg[0].len = 1;
229 msg[0].buf = ®
230
231 msg[1].addr = c->addr;
232 msg[1].flags = I2C_M_RD;
233 msg[1].len = 2;
234 msg[1].buf = (char *)&buf;
235
236 ret = i2c_transfer(c->adapter, msg, 2);
237 if (ret < 0) {
238 dev_err(&c->dev, "i2c read transfer error: %d\n", ret);
239 return ret;
240 }
241
242 *val = be16_to_cpu(buf);
243
244 dev_dbg(&c->dev, "%s: %x=%x\n", __func__, reg, *val);
245
246 return 0;
247 }
248
__mt9v111_write(struct i2c_client * c,u8 reg,u16 val)249 static int __mt9v111_write(struct i2c_client *c, u8 reg, u16 val)
250 {
251 struct i2c_msg msg;
252 u8 buf[3] = { 0 };
253 int ret;
254
255 buf[0] = reg;
256 buf[1] = val >> 8;
257 buf[2] = val & 0xff;
258
259 msg.addr = c->addr;
260 msg.flags = 0;
261 msg.len = 3;
262 msg.buf = (char *)buf;
263
264 dev_dbg(&c->dev, "%s: %x = %x%x\n", __func__, reg, buf[1], buf[2]);
265
266 ret = i2c_transfer(c->adapter, &msg, 1);
267 if (ret < 0) {
268 dev_err(&c->dev, "i2c write transfer error: %d\n", ret);
269 return ret;
270 }
271
272 return 0;
273 }
274
__mt9v111_addr_space_select(struct i2c_client * c,u16 addr_space)275 static int __mt9v111_addr_space_select(struct i2c_client *c, u16 addr_space)
276 {
277 struct v4l2_subdev *sd = i2c_get_clientdata(c);
278 struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
279 u16 val;
280 int ret;
281
282 if (mt9v111->addr_space == addr_space)
283 return 0;
284
285 ret = __mt9v111_write(c, MT9V111_R01_ADDR_SPACE, addr_space);
286 if (ret)
287 return ret;
288
289 /* Verify address space has been updated */
290 ret = __mt9v111_read(c, MT9V111_R01_ADDR_SPACE, &val);
291 if (ret)
292 return ret;
293
294 if (val != addr_space)
295 return -EINVAL;
296
297 mt9v111->addr_space = addr_space;
298
299 return 0;
300 }
301
mt9v111_read(struct i2c_client * c,u8 addr_space,u8 reg,u16 * val)302 static int mt9v111_read(struct i2c_client *c, u8 addr_space, u8 reg, u16 *val)
303 {
304 int ret;
305
306 /* Select register address space first. */
307 ret = __mt9v111_addr_space_select(c, addr_space);
308 if (ret)
309 return ret;
310
311 ret = __mt9v111_read(c, reg, val);
312 if (ret)
313 return ret;
314
315 return 0;
316 }
317
mt9v111_write(struct i2c_client * c,u8 addr_space,u8 reg,u16 val)318 static int mt9v111_write(struct i2c_client *c, u8 addr_space, u8 reg, u16 val)
319 {
320 int ret;
321
322 /* Select register address space first. */
323 ret = __mt9v111_addr_space_select(c, addr_space);
324 if (ret)
325 return ret;
326
327 ret = __mt9v111_write(c, reg, val);
328 if (ret)
329 return ret;
330
331 return 0;
332 }
333
mt9v111_update(struct i2c_client * c,u8 addr_space,u8 reg,u16 mask,u16 val)334 static int mt9v111_update(struct i2c_client *c, u8 addr_space, u8 reg,
335 u16 mask, u16 val)
336 {
337 u16 current_val;
338 int ret;
339
340 /* Select register address space first. */
341 ret = __mt9v111_addr_space_select(c, addr_space);
342 if (ret)
343 return ret;
344
345 /* Read the current register value, then update it. */
346 ret = __mt9v111_read(c, reg, ¤t_val);
347 if (ret)
348 return ret;
349
350 current_val &= ~mask;
351 current_val |= (val & mask);
352 ret = __mt9v111_write(c, reg, current_val);
353 if (ret)
354 return ret;
355
356 return 0;
357 }
358
359 /* --- Sensor HW operations --- */
360
__mt9v111_power_on(struct v4l2_subdev * sd)361 static int __mt9v111_power_on(struct v4l2_subdev *sd)
362 {
363 struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
364 int ret;
365
366 ret = clk_prepare_enable(mt9v111->clk);
367 if (ret)
368 return ret;
369
370 clk_set_rate(mt9v111->clk, mt9v111->sysclk);
371
372 gpiod_set_value(mt9v111->standby, 0);
373 usleep_range(500, 1000);
374
375 gpiod_set_value(mt9v111->oe, 1);
376 usleep_range(500, 1000);
377
378 return 0;
379 }
380
__mt9v111_power_off(struct v4l2_subdev * sd)381 static int __mt9v111_power_off(struct v4l2_subdev *sd)
382 {
383 struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
384
385 gpiod_set_value(mt9v111->oe, 0);
386 usleep_range(500, 1000);
387
388 gpiod_set_value(mt9v111->standby, 1);
389 usleep_range(500, 1000);
390
391 clk_disable_unprepare(mt9v111->clk);
392
393 return 0;
394 }
395
__mt9v111_hw_reset(struct mt9v111_dev * mt9v111)396 static int __mt9v111_hw_reset(struct mt9v111_dev *mt9v111)
397 {
398 if (!mt9v111->reset)
399 return -EINVAL;
400
401 gpiod_set_value(mt9v111->reset, 1);
402 usleep_range(500, 1000);
403
404 gpiod_set_value(mt9v111->reset, 0);
405 usleep_range(500, 1000);
406
407 return 0;
408 }
409
__mt9v111_sw_reset(struct mt9v111_dev * mt9v111)410 static int __mt9v111_sw_reset(struct mt9v111_dev *mt9v111)
411 {
412 struct i2c_client *c = mt9v111->client;
413 int ret;
414
415 /* Software reset core and IFP blocks. */
416
417 ret = mt9v111_update(c, MT9V111_R01_CORE,
418 MT9V111_CORE_R0D_CORE_RESET,
419 MT9V111_CORE_R0D_CORE_RESET_MASK, 1);
420 if (ret)
421 return ret;
422 usleep_range(500, 1000);
423
424 ret = mt9v111_update(c, MT9V111_R01_CORE,
425 MT9V111_CORE_R0D_CORE_RESET,
426 MT9V111_CORE_R0D_CORE_RESET_MASK, 0);
427 if (ret)
428 return ret;
429 usleep_range(500, 1000);
430
431 ret = mt9v111_update(c, MT9V111_R01_IFP,
432 MT9V111_IFP_R07_IFP_RESET,
433 MT9V111_IFP_R07_IFP_RESET_MASK, 1);
434 if (ret)
435 return ret;
436 usleep_range(500, 1000);
437
438 ret = mt9v111_update(c, MT9V111_R01_IFP,
439 MT9V111_IFP_R07_IFP_RESET,
440 MT9V111_IFP_R07_IFP_RESET_MASK, 0);
441 if (ret)
442 return ret;
443 usleep_range(500, 1000);
444
445 return 0;
446 }
447
mt9v111_calc_frame_rate(struct mt9v111_dev * mt9v111,struct v4l2_fract * tpf)448 static int mt9v111_calc_frame_rate(struct mt9v111_dev *mt9v111,
449 struct v4l2_fract *tpf)
450 {
451 unsigned int fps = tpf->numerator ?
452 tpf->denominator / tpf->numerator :
453 tpf->denominator;
454 unsigned int best_diff;
455 unsigned int frm_cols;
456 unsigned int row_pclk;
457 unsigned int best_fps;
458 unsigned int pclk;
459 unsigned int diff;
460 unsigned int idx;
461 unsigned int hb;
462 unsigned int vb;
463 unsigned int i;
464 int ret;
465
466 /* Approximate to the closest supported frame interval. */
467 best_diff = ~0L;
468 for (i = 0, idx = 0; i < ARRAY_SIZE(mt9v111_frame_intervals); i++) {
469 diff = abs(fps - mt9v111_frame_intervals[i]);
470 if (diff < best_diff) {
471 idx = i;
472 best_diff = diff;
473 }
474 }
475 fps = mt9v111_frame_intervals[idx];
476
477 /*
478 * The sensor does not provide a PLL circuitry and pixel clock is
479 * generated dividing the master clock source by two.
480 *
481 * Trow = (W + Hblank + 114) * 2 * (1 / SYSCLK)
482 * TFrame = Trow * (H + Vblank + 2)
483 *
484 * FPS = (SYSCLK / 2) / (Trow * (H + Vblank + 2))
485 *
486 * This boils down to tune H and V blanks to best approximate the
487 * above equation.
488 *
489 * Test all available H/V blank values, until we reach the
490 * desired frame rate.
491 */
492 best_fps = vb = hb = 0;
493 pclk = DIV_ROUND_CLOSEST(mt9v111->sysclk, 2);
494 row_pclk = MT9V111_PIXEL_ARRAY_WIDTH + 7 + MT9V111_CORE_R04_WIN_H_OFFS;
495 frm_cols = MT9V111_PIXEL_ARRAY_HEIGHT + 7 + MT9V111_CORE_R03_WIN_V_OFFS;
496
497 best_diff = ~0L;
498 for (vb = MT9V111_CORE_R06_MIN_VBLANK;
499 vb < MT9V111_CORE_R06_MAX_VBLANK; vb++) {
500 for (hb = MT9V111_CORE_R05_MIN_HBLANK;
501 hb < MT9V111_CORE_R05_MAX_HBLANK; hb += 10) {
502 unsigned int t_frame = (row_pclk + hb) *
503 (frm_cols + vb);
504 unsigned int t_fps = DIV_ROUND_CLOSEST(pclk, t_frame);
505
506 diff = abs(fps - t_fps);
507 if (diff < best_diff) {
508 best_diff = diff;
509 best_fps = t_fps;
510
511 if (diff == 0)
512 break;
513 }
514 }
515
516 if (diff == 0)
517 break;
518 }
519
520 ret = v4l2_ctrl_s_ctrl_int64(mt9v111->hblank, hb);
521 if (ret)
522 return ret;
523
524 ret = v4l2_ctrl_s_ctrl_int64(mt9v111->vblank, vb);
525 if (ret)
526 return ret;
527
528 tpf->numerator = 1;
529 tpf->denominator = best_fps;
530
531 return 0;
532 }
533
mt9v111_hw_config(struct mt9v111_dev * mt9v111)534 static int mt9v111_hw_config(struct mt9v111_dev *mt9v111)
535 {
536 struct i2c_client *c = mt9v111->client;
537 unsigned int ret;
538 u16 outfmtctrl2;
539
540 /* Force device reset. */
541 ret = __mt9v111_hw_reset(mt9v111);
542 if (ret == -EINVAL)
543 ret = __mt9v111_sw_reset(mt9v111);
544 if (ret)
545 return ret;
546
547 /* Configure internal clock sample rate. */
548 ret = mt9v111->sysclk < DIV_ROUND_CLOSEST(MT9V111_MAX_CLKIN, 2) ?
549 mt9v111_update(c, MT9V111_R01_CORE,
550 MT9V111_CORE_R07_OUT_CTRL,
551 MT9V111_CORE_R07_OUT_CTRL_SAMPLE, 1) :
552 mt9v111_update(c, MT9V111_R01_CORE,
553 MT9V111_CORE_R07_OUT_CTRL,
554 MT9V111_CORE_R07_OUT_CTRL_SAMPLE, 0);
555 if (ret)
556 return ret;
557
558 /*
559 * Configure output image format components ordering.
560 *
561 * TODO: IFP block can also output several RGB permutations, we only
562 * support YUYV permutations at the moment.
563 */
564 switch (mt9v111->fmt.code) {
565 case MEDIA_BUS_FMT_YUYV8_2X8:
566 outfmtctrl2 = MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_YC;
567 break;
568 case MEDIA_BUS_FMT_VYUY8_2X8:
569 outfmtctrl2 = MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_CBCR;
570 break;
571 case MEDIA_BUS_FMT_YVYU8_2X8:
572 outfmtctrl2 = MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_YC |
573 MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_CBCR;
574 break;
575 case MEDIA_BUS_FMT_UYVY8_2X8:
576 default:
577 outfmtctrl2 = 0;
578 break;
579 }
580
581 ret = mt9v111_update(c, MT9V111_R01_IFP, MT9V111_IFP_R3A_OUTFMT_CTRL2,
582 MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_MASK,
583 outfmtctrl2);
584 if (ret)
585 return ret;
586
587 /*
588 * Do not change default sensor's core configuration:
589 * output the whole 640x480 pixel array, skip 18 columns and 6 rows.
590 *
591 * Instead, control the output image size through IFP block.
592 *
593 * TODO: No zoom&pan support. Currently we control the output image
594 * size only through decimation, with no zoom support.
595 */
596 ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA5_HPAN,
597 MT9V111_IFP_DECIMATION_FREEZE);
598 if (ret)
599 return ret;
600
601 ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA8_VPAN,
602 MT9V111_IFP_DECIMATION_FREEZE);
603 if (ret)
604 return ret;
605
606 ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA6_HZOOM,
607 MT9V111_IFP_DECIMATION_FREEZE |
608 MT9V111_PIXEL_ARRAY_WIDTH);
609 if (ret)
610 return ret;
611
612 ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA9_VZOOM,
613 MT9V111_IFP_DECIMATION_FREEZE |
614 MT9V111_PIXEL_ARRAY_HEIGHT);
615 if (ret)
616 return ret;
617
618 ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA7_HOUT,
619 MT9V111_IFP_DECIMATION_FREEZE |
620 mt9v111->fmt.width);
621 if (ret)
622 return ret;
623
624 ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RAA_VOUT,
625 mt9v111->fmt.height);
626 if (ret)
627 return ret;
628
629 /* Apply controls to set auto exp, auto awb and timings */
630 ret = v4l2_ctrl_handler_setup(&mt9v111->ctrls);
631 if (ret)
632 return ret;
633
634 /*
635 * Set pixel integration time to the whole frame time.
636 * This value controls the shutter delay when running with AE
637 * disabled. If longer than frame time, it affects the output
638 * frame rate.
639 */
640 return mt9v111_write(c, MT9V111_R01_CORE, MT9V111_CORE_R09_PIXEL_INT,
641 MT9V111_PIXEL_ARRAY_HEIGHT);
642 }
643
644 /* --- V4L2 subdev operations --- */
645
mt9v111_s_power(struct v4l2_subdev * sd,int on)646 static int mt9v111_s_power(struct v4l2_subdev *sd, int on)
647 {
648 struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
649 int pwr_count;
650 int ret = 0;
651
652 mutex_lock(&mt9v111->pwr_mutex);
653
654 /*
655 * Make sure we're transitioning from 0 to 1, or viceversa,
656 * before actually changing the power state.
657 */
658 pwr_count = mt9v111->pwr_count;
659 pwr_count += on ? 1 : -1;
660 if (pwr_count == !!on) {
661 ret = on ? __mt9v111_power_on(sd) :
662 __mt9v111_power_off(sd);
663 if (!ret)
664 /* All went well, updated power counter. */
665 mt9v111->pwr_count = pwr_count;
666
667 mutex_unlock(&mt9v111->pwr_mutex);
668
669 return ret;
670 }
671
672 /*
673 * Update power counter to keep track of how many nested calls we
674 * received.
675 */
676 WARN_ON(pwr_count < 0 || pwr_count > 1);
677 mt9v111->pwr_count = pwr_count;
678
679 mutex_unlock(&mt9v111->pwr_mutex);
680
681 return ret;
682 }
683
mt9v111_s_stream(struct v4l2_subdev * subdev,int enable)684 static int mt9v111_s_stream(struct v4l2_subdev *subdev, int enable)
685 {
686 struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev);
687 int ret;
688
689 mutex_lock(&mt9v111->stream_mutex);
690
691 if (mt9v111->streaming == enable) {
692 mutex_unlock(&mt9v111->stream_mutex);
693 return 0;
694 }
695
696 ret = mt9v111_s_power(subdev, enable);
697 if (ret)
698 goto error_unlock;
699
700 if (enable && mt9v111->pending) {
701 ret = mt9v111_hw_config(mt9v111);
702 if (ret)
703 goto error_unlock;
704
705 /*
706 * No need to update control here as far as only H/VBLANK are
707 * supported and immediately programmed to registers in .s_ctrl
708 */
709
710 mt9v111->pending = false;
711 }
712
713 mt9v111->streaming = enable ? true : false;
714 mutex_unlock(&mt9v111->stream_mutex);
715
716 return 0;
717
718 error_unlock:
719 mutex_unlock(&mt9v111->stream_mutex);
720
721 return ret;
722 }
723
mt9v111_s_frame_interval(struct v4l2_subdev * sd,struct v4l2_subdev_frame_interval * ival)724 static int mt9v111_s_frame_interval(struct v4l2_subdev *sd,
725 struct v4l2_subdev_frame_interval *ival)
726 {
727 struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
728 struct v4l2_fract *tpf = &ival->interval;
729 unsigned int fps = tpf->numerator ?
730 tpf->denominator / tpf->numerator :
731 tpf->denominator;
732 unsigned int max_fps;
733
734 if (!tpf->numerator)
735 tpf->numerator = 1;
736
737 mutex_lock(&mt9v111->stream_mutex);
738
739 if (mt9v111->streaming) {
740 mutex_unlock(&mt9v111->stream_mutex);
741 return -EBUSY;
742 }
743
744 if (mt9v111->fps == fps) {
745 mutex_unlock(&mt9v111->stream_mutex);
746 return 0;
747 }
748
749 /* Make sure frame rate/image sizes constraints are respected. */
750 if (mt9v111->fmt.width < QVGA_WIDTH &&
751 mt9v111->fmt.height < QVGA_HEIGHT)
752 max_fps = 90;
753 else if (mt9v111->fmt.width < CIF_WIDTH &&
754 mt9v111->fmt.height < CIF_HEIGHT)
755 max_fps = 60;
756 else
757 max_fps = mt9v111->sysclk <
758 DIV_ROUND_CLOSEST(MT9V111_MAX_CLKIN, 2) ? 15 :
759 30;
760
761 if (fps > max_fps) {
762 mutex_unlock(&mt9v111->stream_mutex);
763 return -EINVAL;
764 }
765
766 mt9v111_calc_frame_rate(mt9v111, tpf);
767
768 mt9v111->fps = fps;
769 mt9v111->pending = true;
770
771 mutex_unlock(&mt9v111->stream_mutex);
772
773 return 0;
774 }
775
mt9v111_g_frame_interval(struct v4l2_subdev * sd,struct v4l2_subdev_frame_interval * ival)776 static int mt9v111_g_frame_interval(struct v4l2_subdev *sd,
777 struct v4l2_subdev_frame_interval *ival)
778 {
779 struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
780 struct v4l2_fract *tpf = &ival->interval;
781
782 mutex_lock(&mt9v111->stream_mutex);
783
784 tpf->numerator = 1;
785 tpf->denominator = mt9v111->fps;
786
787 mutex_unlock(&mt9v111->stream_mutex);
788
789 return 0;
790 }
791
__mt9v111_get_pad_format(struct mt9v111_dev * mt9v111,struct v4l2_subdev_state * sd_state,unsigned int pad,enum v4l2_subdev_format_whence which)792 static struct v4l2_mbus_framefmt *__mt9v111_get_pad_format(
793 struct mt9v111_dev *mt9v111,
794 struct v4l2_subdev_state *sd_state,
795 unsigned int pad,
796 enum v4l2_subdev_format_whence which)
797 {
798 switch (which) {
799 case V4L2_SUBDEV_FORMAT_TRY:
800 #if IS_ENABLED(CONFIG_VIDEO_V4L2_SUBDEV_API)
801 return v4l2_subdev_get_try_format(&mt9v111->sd, sd_state, pad);
802 #else
803 return &sd_state->pads->try_fmt;
804 #endif
805 case V4L2_SUBDEV_FORMAT_ACTIVE:
806 return &mt9v111->fmt;
807 default:
808 return NULL;
809 }
810 }
811
mt9v111_enum_mbus_code(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_mbus_code_enum * code)812 static int mt9v111_enum_mbus_code(struct v4l2_subdev *subdev,
813 struct v4l2_subdev_state *sd_state,
814 struct v4l2_subdev_mbus_code_enum *code)
815 {
816 if (code->pad || code->index > ARRAY_SIZE(mt9v111_formats) - 1)
817 return -EINVAL;
818
819 code->code = mt9v111_formats[code->index].code;
820
821 return 0;
822 }
823
mt9v111_enum_frame_interval(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_frame_interval_enum * fie)824 static int mt9v111_enum_frame_interval(struct v4l2_subdev *sd,
825 struct v4l2_subdev_state *sd_state,
826 struct v4l2_subdev_frame_interval_enum *fie)
827 {
828 unsigned int i;
829
830 if (fie->pad || fie->index >= ARRAY_SIZE(mt9v111_frame_intervals))
831 return -EINVAL;
832
833 for (i = 0; i < ARRAY_SIZE(mt9v111_frame_sizes); i++)
834 if (fie->width == mt9v111_frame_sizes[i].width &&
835 fie->height == mt9v111_frame_sizes[i].height)
836 break;
837
838 if (i == ARRAY_SIZE(mt9v111_frame_sizes))
839 return -EINVAL;
840
841 fie->interval.numerator = 1;
842 fie->interval.denominator = mt9v111_frame_intervals[fie->index];
843
844 return 0;
845 }
846
mt9v111_enum_frame_size(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_frame_size_enum * fse)847 static int mt9v111_enum_frame_size(struct v4l2_subdev *subdev,
848 struct v4l2_subdev_state *sd_state,
849 struct v4l2_subdev_frame_size_enum *fse)
850 {
851 if (fse->pad || fse->index >= ARRAY_SIZE(mt9v111_frame_sizes))
852 return -EINVAL;
853
854 fse->min_width = mt9v111_frame_sizes[fse->index].width;
855 fse->max_width = mt9v111_frame_sizes[fse->index].width;
856 fse->min_height = mt9v111_frame_sizes[fse->index].height;
857 fse->max_height = mt9v111_frame_sizes[fse->index].height;
858
859 return 0;
860 }
861
mt9v111_get_format(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * format)862 static int mt9v111_get_format(struct v4l2_subdev *subdev,
863 struct v4l2_subdev_state *sd_state,
864 struct v4l2_subdev_format *format)
865 {
866 struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev);
867
868 if (format->pad)
869 return -EINVAL;
870
871 mutex_lock(&mt9v111->stream_mutex);
872 format->format = *__mt9v111_get_pad_format(mt9v111, sd_state,
873 format->pad,
874 format->which);
875 mutex_unlock(&mt9v111->stream_mutex);
876
877 return 0;
878 }
879
mt9v111_set_format(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * format)880 static int mt9v111_set_format(struct v4l2_subdev *subdev,
881 struct v4l2_subdev_state *sd_state,
882 struct v4l2_subdev_format *format)
883 {
884 struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev);
885 struct v4l2_mbus_framefmt new_fmt;
886 struct v4l2_mbus_framefmt *__fmt;
887 unsigned int best_fit = ~0L;
888 unsigned int idx = 0;
889 unsigned int i;
890
891 mutex_lock(&mt9v111->stream_mutex);
892 if (mt9v111->streaming) {
893 mutex_unlock(&mt9v111->stream_mutex);
894 return -EBUSY;
895 }
896
897 if (format->pad) {
898 mutex_unlock(&mt9v111->stream_mutex);
899 return -EINVAL;
900 }
901
902 /* Update mbus format code and sizes. */
903 for (i = 0; i < ARRAY_SIZE(mt9v111_formats); i++) {
904 if (format->format.code == mt9v111_formats[i].code) {
905 new_fmt.code = mt9v111_formats[i].code;
906 break;
907 }
908 }
909 if (i == ARRAY_SIZE(mt9v111_formats))
910 new_fmt.code = mt9v111_formats[0].code;
911
912 for (i = 0; i < ARRAY_SIZE(mt9v111_frame_sizes); i++) {
913 unsigned int fit = abs(mt9v111_frame_sizes[i].width -
914 format->format.width) +
915 abs(mt9v111_frame_sizes[i].height -
916 format->format.height);
917 if (fit < best_fit) {
918 best_fit = fit;
919 idx = i;
920
921 if (fit == 0)
922 break;
923 }
924 }
925 new_fmt.width = mt9v111_frame_sizes[idx].width;
926 new_fmt.height = mt9v111_frame_sizes[idx].height;
927
928 /* Update the device (or pad) format if it has changed. */
929 __fmt = __mt9v111_get_pad_format(mt9v111, sd_state, format->pad,
930 format->which);
931
932 /* Format hasn't changed, stop here. */
933 if (__fmt->code == new_fmt.code &&
934 __fmt->width == new_fmt.width &&
935 __fmt->height == new_fmt.height)
936 goto done;
937
938 /* Update the format and sizes, then mark changes as pending. */
939 __fmt->code = new_fmt.code;
940 __fmt->width = new_fmt.width;
941 __fmt->height = new_fmt.height;
942
943 if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
944 mt9v111->pending = true;
945
946 dev_dbg(mt9v111->dev, "%s: mbus_code: %x - (%ux%u)\n",
947 __func__, __fmt->code, __fmt->width, __fmt->height);
948
949 done:
950 format->format = *__fmt;
951
952 mutex_unlock(&mt9v111->stream_mutex);
953
954 return 0;
955 }
956
mt9v111_init_cfg(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state)957 static int mt9v111_init_cfg(struct v4l2_subdev *subdev,
958 struct v4l2_subdev_state *sd_state)
959 {
960 sd_state->pads->try_fmt = mt9v111_def_fmt;
961
962 return 0;
963 }
964
965 static const struct v4l2_subdev_core_ops mt9v111_core_ops = {
966 .s_power = mt9v111_s_power,
967 };
968
969 static const struct v4l2_subdev_video_ops mt9v111_video_ops = {
970 .s_stream = mt9v111_s_stream,
971 .s_frame_interval = mt9v111_s_frame_interval,
972 .g_frame_interval = mt9v111_g_frame_interval,
973 };
974
975 static const struct v4l2_subdev_pad_ops mt9v111_pad_ops = {
976 .init_cfg = mt9v111_init_cfg,
977 .enum_mbus_code = mt9v111_enum_mbus_code,
978 .enum_frame_size = mt9v111_enum_frame_size,
979 .enum_frame_interval = mt9v111_enum_frame_interval,
980 .get_fmt = mt9v111_get_format,
981 .set_fmt = mt9v111_set_format,
982 };
983
984 static const struct v4l2_subdev_ops mt9v111_ops = {
985 .core = &mt9v111_core_ops,
986 .video = &mt9v111_video_ops,
987 .pad = &mt9v111_pad_ops,
988 };
989
990 #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
991 static const struct media_entity_operations mt9v111_subdev_entity_ops = {
992 .link_validate = v4l2_subdev_link_validate,
993 };
994 #endif
995
996 /* --- V4L2 ctrl --- */
mt9v111_s_ctrl(struct v4l2_ctrl * ctrl)997 static int mt9v111_s_ctrl(struct v4l2_ctrl *ctrl)
998 {
999 struct mt9v111_dev *mt9v111 = container_of(ctrl->handler,
1000 struct mt9v111_dev,
1001 ctrls);
1002 int ret;
1003
1004 mutex_lock(&mt9v111->pwr_mutex);
1005 /*
1006 * If sensor is powered down, just cache new control values,
1007 * no actual register access.
1008 */
1009 if (!mt9v111->pwr_count) {
1010 mt9v111->pending = true;
1011 mutex_unlock(&mt9v111->pwr_mutex);
1012 return 0;
1013 }
1014 mutex_unlock(&mt9v111->pwr_mutex);
1015
1016 /*
1017 * Flickering control gets disabled if both auto exp and auto awb
1018 * are disabled too. If any of the two is enabled, enable it.
1019 *
1020 * Disabling flickering when ae and awb are off allows a more precise
1021 * control of the programmed frame rate.
1022 */
1023 if (mt9v111->auto_exp->is_new || mt9v111->auto_awb->is_new) {
1024 if (mt9v111->auto_exp->val == V4L2_EXPOSURE_MANUAL &&
1025 mt9v111->auto_awb->val == V4L2_WHITE_BALANCE_MANUAL)
1026 ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
1027 MT9V111_IFP_R08_OUTFMT_CTRL,
1028 MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER,
1029 0);
1030 else
1031 ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
1032 MT9V111_IFP_R08_OUTFMT_CTRL,
1033 MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER,
1034 1);
1035 if (ret)
1036 return ret;
1037 }
1038
1039 ret = -EINVAL;
1040 switch (ctrl->id) {
1041 case V4L2_CID_AUTO_WHITE_BALANCE:
1042 ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
1043 MT9V111_IFP_R06_OPMODE_CTRL,
1044 MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN,
1045 ctrl->val == V4L2_WHITE_BALANCE_AUTO ?
1046 MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN : 0);
1047 break;
1048 case V4L2_CID_EXPOSURE_AUTO:
1049 ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
1050 MT9V111_IFP_R06_OPMODE_CTRL,
1051 MT9V111_IFP_R06_OPMODE_CTRL_AE_EN,
1052 ctrl->val == V4L2_EXPOSURE_AUTO ?
1053 MT9V111_IFP_R06_OPMODE_CTRL_AE_EN : 0);
1054 break;
1055 case V4L2_CID_HBLANK:
1056 ret = mt9v111_update(mt9v111->client, MT9V111_R01_CORE,
1057 MT9V111_CORE_R05_HBLANK,
1058 MT9V111_CORE_R05_MAX_HBLANK,
1059 mt9v111->hblank->val);
1060 break;
1061 case V4L2_CID_VBLANK:
1062 ret = mt9v111_update(mt9v111->client, MT9V111_R01_CORE,
1063 MT9V111_CORE_R06_VBLANK,
1064 MT9V111_CORE_R06_MAX_VBLANK,
1065 mt9v111->vblank->val);
1066 break;
1067 }
1068
1069 return ret;
1070 }
1071
1072 static const struct v4l2_ctrl_ops mt9v111_ctrl_ops = {
1073 .s_ctrl = mt9v111_s_ctrl,
1074 };
1075
mt9v111_chip_probe(struct mt9v111_dev * mt9v111)1076 static int mt9v111_chip_probe(struct mt9v111_dev *mt9v111)
1077 {
1078 int ret;
1079 u16 val;
1080
1081 ret = __mt9v111_power_on(&mt9v111->sd);
1082 if (ret)
1083 return ret;
1084
1085 ret = mt9v111_read(mt9v111->client, MT9V111_R01_CORE,
1086 MT9V111_CORE_RFF_CHIP_VER, &val);
1087 if (ret)
1088 goto power_off;
1089
1090 if ((val >> 8) != MT9V111_CHIP_ID_HIGH &&
1091 (val & 0xff) != MT9V111_CHIP_ID_LOW) {
1092 dev_err(mt9v111->dev,
1093 "Unable to identify MT9V111 chip: 0x%2x%2x\n",
1094 val >> 8, val & 0xff);
1095 ret = -EIO;
1096 goto power_off;
1097 }
1098
1099 dev_dbg(mt9v111->dev, "Chip identified: 0x%2x%2x\n",
1100 val >> 8, val & 0xff);
1101
1102 power_off:
1103 __mt9v111_power_off(&mt9v111->sd);
1104
1105 return ret;
1106 }
1107
mt9v111_probe(struct i2c_client * client)1108 static int mt9v111_probe(struct i2c_client *client)
1109 {
1110 struct mt9v111_dev *mt9v111;
1111 struct v4l2_fract tpf;
1112 int ret;
1113
1114 mt9v111 = devm_kzalloc(&client->dev, sizeof(*mt9v111), GFP_KERNEL);
1115 if (!mt9v111)
1116 return -ENOMEM;
1117
1118 mt9v111->dev = &client->dev;
1119 mt9v111->client = client;
1120
1121 mt9v111->clk = devm_clk_get(&client->dev, NULL);
1122 if (IS_ERR(mt9v111->clk))
1123 return PTR_ERR(mt9v111->clk);
1124
1125 mt9v111->sysclk = clk_get_rate(mt9v111->clk);
1126 if (mt9v111->sysclk > MT9V111_MAX_CLKIN)
1127 return -EINVAL;
1128
1129 mt9v111->oe = devm_gpiod_get_optional(&client->dev, "enable",
1130 GPIOD_OUT_LOW);
1131 if (IS_ERR(mt9v111->oe)) {
1132 dev_err(&client->dev, "Unable to get GPIO \"enable\": %ld\n",
1133 PTR_ERR(mt9v111->oe));
1134 return PTR_ERR(mt9v111->oe);
1135 }
1136
1137 mt9v111->standby = devm_gpiod_get_optional(&client->dev, "standby",
1138 GPIOD_OUT_HIGH);
1139 if (IS_ERR(mt9v111->standby)) {
1140 dev_err(&client->dev, "Unable to get GPIO \"standby\": %ld\n",
1141 PTR_ERR(mt9v111->standby));
1142 return PTR_ERR(mt9v111->standby);
1143 }
1144
1145 mt9v111->reset = devm_gpiod_get_optional(&client->dev, "reset",
1146 GPIOD_OUT_LOW);
1147 if (IS_ERR(mt9v111->reset)) {
1148 dev_err(&client->dev, "Unable to get GPIO \"reset\": %ld\n",
1149 PTR_ERR(mt9v111->reset));
1150 return PTR_ERR(mt9v111->reset);
1151 }
1152
1153 mutex_init(&mt9v111->pwr_mutex);
1154 mutex_init(&mt9v111->stream_mutex);
1155
1156 v4l2_ctrl_handler_init(&mt9v111->ctrls, 5);
1157
1158 mt9v111->auto_awb = v4l2_ctrl_new_std(&mt9v111->ctrls,
1159 &mt9v111_ctrl_ops,
1160 V4L2_CID_AUTO_WHITE_BALANCE,
1161 0, 1, 1,
1162 V4L2_WHITE_BALANCE_AUTO);
1163 mt9v111->auto_exp = v4l2_ctrl_new_std_menu(&mt9v111->ctrls,
1164 &mt9v111_ctrl_ops,
1165 V4L2_CID_EXPOSURE_AUTO,
1166 V4L2_EXPOSURE_MANUAL,
1167 0, V4L2_EXPOSURE_AUTO);
1168 mt9v111->hblank = v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops,
1169 V4L2_CID_HBLANK,
1170 MT9V111_CORE_R05_MIN_HBLANK,
1171 MT9V111_CORE_R05_MAX_HBLANK, 1,
1172 MT9V111_CORE_R05_DEF_HBLANK);
1173 mt9v111->vblank = v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops,
1174 V4L2_CID_VBLANK,
1175 MT9V111_CORE_R06_MIN_VBLANK,
1176 MT9V111_CORE_R06_MAX_VBLANK, 1,
1177 MT9V111_CORE_R06_DEF_VBLANK);
1178
1179 /* PIXEL_RATE is fixed: just expose it to user space. */
1180 v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops,
1181 V4L2_CID_PIXEL_RATE, 0,
1182 DIV_ROUND_CLOSEST(mt9v111->sysclk, 2), 1,
1183 DIV_ROUND_CLOSEST(mt9v111->sysclk, 2));
1184
1185 if (mt9v111->ctrls.error) {
1186 ret = mt9v111->ctrls.error;
1187 goto error_free_ctrls;
1188 }
1189 mt9v111->sd.ctrl_handler = &mt9v111->ctrls;
1190
1191 /* Start with default configuration: 640x480 UYVY. */
1192 mt9v111->fmt = mt9v111_def_fmt;
1193
1194 /* Re-calculate blankings for 640x480@15fps. */
1195 mt9v111->fps = 15;
1196 tpf.numerator = 1;
1197 tpf.denominator = mt9v111->fps;
1198 mt9v111_calc_frame_rate(mt9v111, &tpf);
1199
1200 mt9v111->pwr_count = 0;
1201 mt9v111->addr_space = MT9V111_R01_IFP;
1202 mt9v111->pending = true;
1203
1204 v4l2_i2c_subdev_init(&mt9v111->sd, client, &mt9v111_ops);
1205
1206 #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
1207 mt9v111->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1208 mt9v111->sd.entity.ops = &mt9v111_subdev_entity_ops;
1209 mt9v111->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1210
1211 mt9v111->pad.flags = MEDIA_PAD_FL_SOURCE;
1212 ret = media_entity_pads_init(&mt9v111->sd.entity, 1, &mt9v111->pad);
1213 if (ret)
1214 goto error_free_entity;
1215 #endif
1216
1217 ret = mt9v111_chip_probe(mt9v111);
1218 if (ret)
1219 goto error_free_entity;
1220
1221 ret = v4l2_async_register_subdev(&mt9v111->sd);
1222 if (ret)
1223 goto error_free_entity;
1224
1225 return 0;
1226
1227 error_free_entity:
1228 #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
1229 media_entity_cleanup(&mt9v111->sd.entity);
1230 #endif
1231
1232 error_free_ctrls:
1233 v4l2_ctrl_handler_free(&mt9v111->ctrls);
1234
1235 mutex_destroy(&mt9v111->pwr_mutex);
1236 mutex_destroy(&mt9v111->stream_mutex);
1237
1238 return ret;
1239 }
1240
mt9v111_remove(struct i2c_client * client)1241 static void mt9v111_remove(struct i2c_client *client)
1242 {
1243 struct v4l2_subdev *sd = i2c_get_clientdata(client);
1244 struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
1245
1246 v4l2_async_unregister_subdev(sd);
1247
1248 #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER)
1249 media_entity_cleanup(&sd->entity);
1250 #endif
1251
1252 v4l2_ctrl_handler_free(&mt9v111->ctrls);
1253
1254 mutex_destroy(&mt9v111->pwr_mutex);
1255 mutex_destroy(&mt9v111->stream_mutex);
1256 }
1257
1258 static const struct of_device_id mt9v111_of_match[] = {
1259 { .compatible = "aptina,mt9v111", },
1260 { /* sentinel */ },
1261 };
1262
1263 static struct i2c_driver mt9v111_driver = {
1264 .driver = {
1265 .name = "mt9v111",
1266 .of_match_table = mt9v111_of_match,
1267 },
1268 .probe_new = mt9v111_probe,
1269 .remove = mt9v111_remove,
1270 };
1271
1272 module_i2c_driver(mt9v111_driver);
1273
1274 MODULE_DESCRIPTION("V4L2 sensor driver for Aptina MT9V111");
1275 MODULE_AUTHOR("Jacopo Mondi <jacopo@jmondi.org>");
1276 MODULE_LICENSE("GPL v2");
1277