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