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 clk_set_rate(mt9v111->clk, mt9v111->sysclk);
369
370 gpiod_set_value(mt9v111->standby, 0);
371 usleep_range(500, 1000);
372
373 gpiod_set_value(mt9v111->oe, 1);
374 usleep_range(500, 1000);
375
376 return 0;
377 }
378
__mt9v111_power_off(struct v4l2_subdev * sd)379 static int __mt9v111_power_off(struct v4l2_subdev *sd)
380 {
381 struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
382
383 gpiod_set_value(mt9v111->oe, 0);
384 usleep_range(500, 1000);
385
386 gpiod_set_value(mt9v111->standby, 1);
387 usleep_range(500, 1000);
388
389 clk_disable_unprepare(mt9v111->clk);
390
391 return 0;
392 }
393
__mt9v111_hw_reset(struct mt9v111_dev * mt9v111)394 static int __mt9v111_hw_reset(struct mt9v111_dev *mt9v111)
395 {
396 if (!mt9v111->reset)
397 return -EINVAL;
398
399 gpiod_set_value(mt9v111->reset, 1);
400 usleep_range(500, 1000);
401
402 gpiod_set_value(mt9v111->reset, 0);
403 usleep_range(500, 1000);
404
405 return 0;
406 }
407
__mt9v111_sw_reset(struct mt9v111_dev * mt9v111)408 static int __mt9v111_sw_reset(struct mt9v111_dev *mt9v111)
409 {
410 struct i2c_client *c = mt9v111->client;
411 int ret;
412
413 /* Software reset core and IFP blocks. */
414
415 ret = mt9v111_update(c, MT9V111_R01_CORE,
416 MT9V111_CORE_R0D_CORE_RESET,
417 MT9V111_CORE_R0D_CORE_RESET_MASK, 1);
418 if (ret)
419 return ret;
420 usleep_range(500, 1000);
421
422 ret = mt9v111_update(c, MT9V111_R01_CORE,
423 MT9V111_CORE_R0D_CORE_RESET,
424 MT9V111_CORE_R0D_CORE_RESET_MASK, 0);
425 if (ret)
426 return ret;
427 usleep_range(500, 1000);
428
429 ret = mt9v111_update(c, MT9V111_R01_IFP,
430 MT9V111_IFP_R07_IFP_RESET,
431 MT9V111_IFP_R07_IFP_RESET_MASK, 1);
432 if (ret)
433 return ret;
434 usleep_range(500, 1000);
435
436 ret = mt9v111_update(c, MT9V111_R01_IFP,
437 MT9V111_IFP_R07_IFP_RESET,
438 MT9V111_IFP_R07_IFP_RESET_MASK, 0);
439 if (ret)
440 return ret;
441 usleep_range(500, 1000);
442
443 return 0;
444 }
445
mt9v111_calc_frame_rate(struct mt9v111_dev * mt9v111,struct v4l2_fract * tpf)446 static int mt9v111_calc_frame_rate(struct mt9v111_dev *mt9v111,
447 struct v4l2_fract *tpf)
448 {
449 unsigned int fps = tpf->numerator ?
450 tpf->denominator / tpf->numerator :
451 tpf->denominator;
452 unsigned int best_diff;
453 unsigned int frm_cols;
454 unsigned int row_pclk;
455 unsigned int best_fps;
456 unsigned int pclk;
457 unsigned int diff;
458 unsigned int idx;
459 unsigned int hb;
460 unsigned int vb;
461 unsigned int i;
462 int ret;
463
464 /* Approximate to the closest supported frame interval. */
465 best_diff = ~0L;
466 for (i = 0, idx = 0; i < ARRAY_SIZE(mt9v111_frame_intervals); i++) {
467 diff = abs(fps - mt9v111_frame_intervals[i]);
468 if (diff < best_diff) {
469 idx = i;
470 best_diff = diff;
471 }
472 }
473 fps = mt9v111_frame_intervals[idx];
474
475 /*
476 * The sensor does not provide a PLL circuitry and pixel clock is
477 * generated dividing the master clock source by two.
478 *
479 * Trow = (W + Hblank + 114) * 2 * (1 / SYSCLK)
480 * TFrame = Trow * (H + Vblank + 2)
481 *
482 * FPS = (SYSCLK / 2) / (Trow * (H + Vblank + 2))
483 *
484 * This boils down to tune H and V blanks to best approximate the
485 * above equation.
486 *
487 * Test all available H/V blank values, until we reach the
488 * desired frame rate.
489 */
490 best_fps = vb = hb = 0;
491 pclk = DIV_ROUND_CLOSEST(mt9v111->sysclk, 2);
492 row_pclk = MT9V111_PIXEL_ARRAY_WIDTH + 7 + MT9V111_CORE_R04_WIN_H_OFFS;
493 frm_cols = MT9V111_PIXEL_ARRAY_HEIGHT + 7 + MT9V111_CORE_R03_WIN_V_OFFS;
494
495 best_diff = ~0L;
496 for (vb = MT9V111_CORE_R06_MIN_VBLANK;
497 vb < MT9V111_CORE_R06_MAX_VBLANK; vb++) {
498 for (hb = MT9V111_CORE_R05_MIN_HBLANK;
499 hb < MT9V111_CORE_R05_MAX_HBLANK; hb += 10) {
500 unsigned int t_frame = (row_pclk + hb) *
501 (frm_cols + vb);
502 unsigned int t_fps = DIV_ROUND_CLOSEST(pclk, t_frame);
503
504 diff = abs(fps - t_fps);
505 if (diff < best_diff) {
506 best_diff = diff;
507 best_fps = t_fps;
508
509 if (diff == 0)
510 break;
511 }
512 }
513
514 if (diff == 0)
515 break;
516 }
517
518 ret = v4l2_ctrl_s_ctrl_int64(mt9v111->hblank, hb);
519 if (ret)
520 return ret;
521
522 ret = v4l2_ctrl_s_ctrl_int64(mt9v111->vblank, vb);
523 if (ret)
524 return ret;
525
526 tpf->numerator = 1;
527 tpf->denominator = best_fps;
528
529 return 0;
530 }
531
mt9v111_hw_config(struct mt9v111_dev * mt9v111)532 static int mt9v111_hw_config(struct mt9v111_dev *mt9v111)
533 {
534 struct i2c_client *c = mt9v111->client;
535 unsigned int ret;
536 u16 outfmtctrl2;
537
538 /* Force device reset. */
539 ret = __mt9v111_hw_reset(mt9v111);
540 if (ret == -EINVAL)
541 ret = __mt9v111_sw_reset(mt9v111);
542 if (ret)
543 return ret;
544
545 /* Configure internal clock sample rate. */
546 ret = mt9v111->sysclk < DIV_ROUND_CLOSEST(MT9V111_MAX_CLKIN, 2) ?
547 mt9v111_update(c, MT9V111_R01_CORE,
548 MT9V111_CORE_R07_OUT_CTRL,
549 MT9V111_CORE_R07_OUT_CTRL_SAMPLE, 1) :
550 mt9v111_update(c, MT9V111_R01_CORE,
551 MT9V111_CORE_R07_OUT_CTRL,
552 MT9V111_CORE_R07_OUT_CTRL_SAMPLE, 0);
553 if (ret)
554 return ret;
555
556 /*
557 * Configure output image format components ordering.
558 *
559 * TODO: IFP block can also output several RGB permutations, we only
560 * support YUYV permutations at the moment.
561 */
562 switch (mt9v111->fmt.code) {
563 case MEDIA_BUS_FMT_YUYV8_2X8:
564 outfmtctrl2 = MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_YC;
565 break;
566 case MEDIA_BUS_FMT_VYUY8_2X8:
567 outfmtctrl2 = MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_CBCR;
568 break;
569 case MEDIA_BUS_FMT_YVYU8_2X8:
570 outfmtctrl2 = MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_YC |
571 MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_CBCR;
572 break;
573 case MEDIA_BUS_FMT_UYVY8_2X8:
574 default:
575 outfmtctrl2 = 0;
576 break;
577 }
578
579 ret = mt9v111_update(c, MT9V111_R01_IFP, MT9V111_IFP_R3A_OUTFMT_CTRL2,
580 MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_MASK,
581 outfmtctrl2);
582 if (ret)
583 return ret;
584
585 /*
586 * Do not change default sensor's core configuration:
587 * output the whole 640x480 pixel array, skip 18 columns and 6 rows.
588 *
589 * Instead, control the output image size through IFP block.
590 *
591 * TODO: No zoom&pan support. Currently we control the output image
592 * size only through decimation, with no zoom support.
593 */
594 ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA5_HPAN,
595 MT9V111_IFP_DECIMATION_FREEZE);
596 if (ret)
597 return ret;
598
599 ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA8_VPAN,
600 MT9V111_IFP_DECIMATION_FREEZE);
601 if (ret)
602 return ret;
603
604 ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA6_HZOOM,
605 MT9V111_IFP_DECIMATION_FREEZE |
606 MT9V111_PIXEL_ARRAY_WIDTH);
607 if (ret)
608 return ret;
609
610 ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA9_VZOOM,
611 MT9V111_IFP_DECIMATION_FREEZE |
612 MT9V111_PIXEL_ARRAY_HEIGHT);
613 if (ret)
614 return ret;
615
616 ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RA7_HOUT,
617 MT9V111_IFP_DECIMATION_FREEZE |
618 mt9v111->fmt.width);
619 if (ret)
620 return ret;
621
622 ret = mt9v111_write(c, MT9V111_R01_IFP, MT9V111_IFP_RAA_VOUT,
623 mt9v111->fmt.height);
624 if (ret)
625 return ret;
626
627 /* Apply controls to set auto exp, auto awb and timings */
628 ret = v4l2_ctrl_handler_setup(&mt9v111->ctrls);
629 if (ret)
630 return ret;
631
632 /*
633 * Set pixel integration time to the whole frame time.
634 * This value controls the shutter delay when running with AE
635 * disabled. If longer than frame time, it affects the output
636 * frame rate.
637 */
638 return mt9v111_write(c, MT9V111_R01_CORE, MT9V111_CORE_R09_PIXEL_INT,
639 MT9V111_PIXEL_ARRAY_HEIGHT);
640 }
641
642 /* --- V4L2 subdev operations --- */
643
mt9v111_s_power(struct v4l2_subdev * sd,int on)644 static int mt9v111_s_power(struct v4l2_subdev *sd, int on)
645 {
646 struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
647 int pwr_count;
648 int ret = 0;
649
650 mutex_lock(&mt9v111->pwr_mutex);
651
652 /*
653 * Make sure we're transitioning from 0 to 1, or viceversa,
654 * before actually changing the power state.
655 */
656 pwr_count = mt9v111->pwr_count;
657 pwr_count += on ? 1 : -1;
658 if (pwr_count == !!on) {
659 ret = on ? __mt9v111_power_on(sd) :
660 __mt9v111_power_off(sd);
661 if (!ret)
662 /* All went well, updated power counter. */
663 mt9v111->pwr_count = pwr_count;
664
665 mutex_unlock(&mt9v111->pwr_mutex);
666
667 return ret;
668 }
669
670 /*
671 * Update power counter to keep track of how many nested calls we
672 * received.
673 */
674 WARN_ON(pwr_count < 0 || pwr_count > 1);
675 mt9v111->pwr_count = pwr_count;
676
677 mutex_unlock(&mt9v111->pwr_mutex);
678
679 return ret;
680 }
681
mt9v111_s_stream(struct v4l2_subdev * subdev,int enable)682 static int mt9v111_s_stream(struct v4l2_subdev *subdev, int enable)
683 {
684 struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev);
685 int ret;
686
687 mutex_lock(&mt9v111->stream_mutex);
688
689 if (mt9v111->streaming == enable) {
690 mutex_unlock(&mt9v111->stream_mutex);
691 return 0;
692 }
693
694 ret = mt9v111_s_power(subdev, enable);
695 if (ret)
696 goto error_unlock;
697
698 if (enable && mt9v111->pending) {
699 ret = mt9v111_hw_config(mt9v111);
700 if (ret)
701 goto error_unlock;
702
703 /*
704 * No need to update control here as far as only H/VBLANK are
705 * supported and immediately programmed to registers in .s_ctrl
706 */
707
708 mt9v111->pending = false;
709 }
710
711 mt9v111->streaming = enable ? true : false;
712 mutex_unlock(&mt9v111->stream_mutex);
713
714 return 0;
715
716 error_unlock:
717 mutex_unlock(&mt9v111->stream_mutex);
718
719 return ret;
720 }
721
mt9v111_set_frame_interval(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_frame_interval * ival)722 static int mt9v111_set_frame_interval(struct v4l2_subdev *sd,
723 struct v4l2_subdev_state *sd_state,
724 struct v4l2_subdev_frame_interval *ival)
725 {
726 struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
727 struct v4l2_fract *tpf = &ival->interval;
728 unsigned int fps = tpf->numerator ?
729 tpf->denominator / tpf->numerator :
730 tpf->denominator;
731 unsigned int max_fps;
732
733 /*
734 * FIXME: Implement support for V4L2_SUBDEV_FORMAT_TRY, using the V4L2
735 * subdev active state API.
736 */
737 if (ival->which != V4L2_SUBDEV_FORMAT_ACTIVE)
738 return -EINVAL;
739
740 if (!tpf->numerator)
741 tpf->numerator = 1;
742
743 mutex_lock(&mt9v111->stream_mutex);
744
745 if (mt9v111->streaming) {
746 mutex_unlock(&mt9v111->stream_mutex);
747 return -EBUSY;
748 }
749
750 if (mt9v111->fps == fps) {
751 mutex_unlock(&mt9v111->stream_mutex);
752 return 0;
753 }
754
755 /* Make sure frame rate/image sizes constraints are respected. */
756 if (mt9v111->fmt.width < QVGA_WIDTH &&
757 mt9v111->fmt.height < QVGA_HEIGHT)
758 max_fps = 90;
759 else if (mt9v111->fmt.width < CIF_WIDTH &&
760 mt9v111->fmt.height < CIF_HEIGHT)
761 max_fps = 60;
762 else
763 max_fps = mt9v111->sysclk <
764 DIV_ROUND_CLOSEST(MT9V111_MAX_CLKIN, 2) ? 15 :
765 30;
766
767 if (fps > max_fps) {
768 mutex_unlock(&mt9v111->stream_mutex);
769 return -EINVAL;
770 }
771
772 mt9v111_calc_frame_rate(mt9v111, tpf);
773
774 mt9v111->fps = fps;
775 mt9v111->pending = true;
776
777 mutex_unlock(&mt9v111->stream_mutex);
778
779 return 0;
780 }
781
mt9v111_get_frame_interval(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_frame_interval * ival)782 static int mt9v111_get_frame_interval(struct v4l2_subdev *sd,
783 struct v4l2_subdev_state *sd_state,
784 struct v4l2_subdev_frame_interval *ival)
785 {
786 struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
787 struct v4l2_fract *tpf = &ival->interval;
788
789 /*
790 * FIXME: Implement support for V4L2_SUBDEV_FORMAT_TRY, using the V4L2
791 * subdev active state API.
792 */
793 if (ival->which != V4L2_SUBDEV_FORMAT_ACTIVE)
794 return -EINVAL;
795
796 mutex_lock(&mt9v111->stream_mutex);
797
798 tpf->numerator = 1;
799 tpf->denominator = mt9v111->fps;
800
801 mutex_unlock(&mt9v111->stream_mutex);
802
803 return 0;
804 }
805
__mt9v111_get_pad_format(struct mt9v111_dev * mt9v111,struct v4l2_subdev_state * sd_state,unsigned int pad,enum v4l2_subdev_format_whence which)806 static struct v4l2_mbus_framefmt *__mt9v111_get_pad_format(
807 struct mt9v111_dev *mt9v111,
808 struct v4l2_subdev_state *sd_state,
809 unsigned int pad,
810 enum v4l2_subdev_format_whence which)
811 {
812 switch (which) {
813 case V4L2_SUBDEV_FORMAT_TRY:
814 return v4l2_subdev_state_get_format(sd_state, pad);
815 case V4L2_SUBDEV_FORMAT_ACTIVE:
816 return &mt9v111->fmt;
817 default:
818 return NULL;
819 }
820 }
821
mt9v111_enum_mbus_code(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_mbus_code_enum * code)822 static int mt9v111_enum_mbus_code(struct v4l2_subdev *subdev,
823 struct v4l2_subdev_state *sd_state,
824 struct v4l2_subdev_mbus_code_enum *code)
825 {
826 if (code->pad || code->index > ARRAY_SIZE(mt9v111_formats) - 1)
827 return -EINVAL;
828
829 code->code = mt9v111_formats[code->index].code;
830
831 return 0;
832 }
833
mt9v111_enum_frame_interval(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_frame_interval_enum * fie)834 static int mt9v111_enum_frame_interval(struct v4l2_subdev *sd,
835 struct v4l2_subdev_state *sd_state,
836 struct v4l2_subdev_frame_interval_enum *fie)
837 {
838 unsigned int i;
839
840 if (fie->pad || fie->index >= ARRAY_SIZE(mt9v111_frame_intervals))
841 return -EINVAL;
842
843 for (i = 0; i < ARRAY_SIZE(mt9v111_frame_sizes); i++)
844 if (fie->width == mt9v111_frame_sizes[i].width &&
845 fie->height == mt9v111_frame_sizes[i].height)
846 break;
847
848 if (i == ARRAY_SIZE(mt9v111_frame_sizes))
849 return -EINVAL;
850
851 fie->interval.numerator = 1;
852 fie->interval.denominator = mt9v111_frame_intervals[fie->index];
853
854 return 0;
855 }
856
mt9v111_enum_frame_size(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_frame_size_enum * fse)857 static int mt9v111_enum_frame_size(struct v4l2_subdev *subdev,
858 struct v4l2_subdev_state *sd_state,
859 struct v4l2_subdev_frame_size_enum *fse)
860 {
861 if (fse->pad || fse->index >= ARRAY_SIZE(mt9v111_frame_sizes))
862 return -EINVAL;
863
864 fse->min_width = mt9v111_frame_sizes[fse->index].width;
865 fse->max_width = mt9v111_frame_sizes[fse->index].width;
866 fse->min_height = mt9v111_frame_sizes[fse->index].height;
867 fse->max_height = mt9v111_frame_sizes[fse->index].height;
868
869 return 0;
870 }
871
mt9v111_get_format(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * format)872 static int mt9v111_get_format(struct v4l2_subdev *subdev,
873 struct v4l2_subdev_state *sd_state,
874 struct v4l2_subdev_format *format)
875 {
876 struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev);
877
878 if (format->pad)
879 return -EINVAL;
880
881 mutex_lock(&mt9v111->stream_mutex);
882 format->format = *__mt9v111_get_pad_format(mt9v111, sd_state,
883 format->pad,
884 format->which);
885 mutex_unlock(&mt9v111->stream_mutex);
886
887 return 0;
888 }
889
mt9v111_set_format(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * format)890 static int mt9v111_set_format(struct v4l2_subdev *subdev,
891 struct v4l2_subdev_state *sd_state,
892 struct v4l2_subdev_format *format)
893 {
894 struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev);
895 struct v4l2_mbus_framefmt new_fmt;
896 struct v4l2_mbus_framefmt *__fmt;
897 unsigned int best_fit = ~0L;
898 unsigned int idx = 0;
899 unsigned int i;
900
901 mutex_lock(&mt9v111->stream_mutex);
902 if (mt9v111->streaming) {
903 mutex_unlock(&mt9v111->stream_mutex);
904 return -EBUSY;
905 }
906
907 if (format->pad) {
908 mutex_unlock(&mt9v111->stream_mutex);
909 return -EINVAL;
910 }
911
912 /* Update mbus format code and sizes. */
913 for (i = 0; i < ARRAY_SIZE(mt9v111_formats); i++) {
914 if (format->format.code == mt9v111_formats[i].code) {
915 new_fmt.code = mt9v111_formats[i].code;
916 break;
917 }
918 }
919 if (i == ARRAY_SIZE(mt9v111_formats))
920 new_fmt.code = mt9v111_formats[0].code;
921
922 for (i = 0; i < ARRAY_SIZE(mt9v111_frame_sizes); i++) {
923 unsigned int fit = abs(mt9v111_frame_sizes[i].width -
924 format->format.width) +
925 abs(mt9v111_frame_sizes[i].height -
926 format->format.height);
927 if (fit < best_fit) {
928 best_fit = fit;
929 idx = i;
930
931 if (fit == 0)
932 break;
933 }
934 }
935 new_fmt.width = mt9v111_frame_sizes[idx].width;
936 new_fmt.height = mt9v111_frame_sizes[idx].height;
937
938 /* Update the device (or pad) format if it has changed. */
939 __fmt = __mt9v111_get_pad_format(mt9v111, sd_state, format->pad,
940 format->which);
941
942 /* Format hasn't changed, stop here. */
943 if (__fmt->code == new_fmt.code &&
944 __fmt->width == new_fmt.width &&
945 __fmt->height == new_fmt.height)
946 goto done;
947
948 /* Update the format and sizes, then mark changes as pending. */
949 __fmt->code = new_fmt.code;
950 __fmt->width = new_fmt.width;
951 __fmt->height = new_fmt.height;
952
953 if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE)
954 mt9v111->pending = true;
955
956 dev_dbg(mt9v111->dev, "%s: mbus_code: %x - (%ux%u)\n",
957 __func__, __fmt->code, __fmt->width, __fmt->height);
958
959 done:
960 format->format = *__fmt;
961
962 mutex_unlock(&mt9v111->stream_mutex);
963
964 return 0;
965 }
966
mt9v111_init_state(struct v4l2_subdev * subdev,struct v4l2_subdev_state * sd_state)967 static int mt9v111_init_state(struct v4l2_subdev *subdev,
968 struct v4l2_subdev_state *sd_state)
969 {
970 *v4l2_subdev_state_get_format(sd_state, 0) = mt9v111_def_fmt;
971
972 return 0;
973 }
974
975 static const struct v4l2_subdev_core_ops mt9v111_core_ops = {
976 .s_power = mt9v111_s_power,
977 };
978
979 static const struct v4l2_subdev_video_ops mt9v111_video_ops = {
980 .s_stream = mt9v111_s_stream,
981 };
982
983 static const struct v4l2_subdev_pad_ops mt9v111_pad_ops = {
984 .enum_mbus_code = mt9v111_enum_mbus_code,
985 .enum_frame_size = mt9v111_enum_frame_size,
986 .enum_frame_interval = mt9v111_enum_frame_interval,
987 .get_fmt = mt9v111_get_format,
988 .set_fmt = mt9v111_set_format,
989 .get_frame_interval = mt9v111_get_frame_interval,
990 .set_frame_interval = mt9v111_set_frame_interval,
991 };
992
993 static const struct v4l2_subdev_ops mt9v111_ops = {
994 .core = &mt9v111_core_ops,
995 .video = &mt9v111_video_ops,
996 .pad = &mt9v111_pad_ops,
997 };
998
999 static const struct v4l2_subdev_internal_ops mt9v111_internal_ops = {
1000 .init_state = mt9v111_init_state,
1001 };
1002
1003 static const struct media_entity_operations mt9v111_subdev_entity_ops = {
1004 .link_validate = v4l2_subdev_link_validate,
1005 };
1006
1007 /* --- V4L2 ctrl --- */
mt9v111_s_ctrl(struct v4l2_ctrl * ctrl)1008 static int mt9v111_s_ctrl(struct v4l2_ctrl *ctrl)
1009 {
1010 struct mt9v111_dev *mt9v111 = container_of(ctrl->handler,
1011 struct mt9v111_dev,
1012 ctrls);
1013 int ret;
1014
1015 mutex_lock(&mt9v111->pwr_mutex);
1016 /*
1017 * If sensor is powered down, just cache new control values,
1018 * no actual register access.
1019 */
1020 if (!mt9v111->pwr_count) {
1021 mt9v111->pending = true;
1022 mutex_unlock(&mt9v111->pwr_mutex);
1023 return 0;
1024 }
1025 mutex_unlock(&mt9v111->pwr_mutex);
1026
1027 /*
1028 * Flickering control gets disabled if both auto exp and auto awb
1029 * are disabled too. If any of the two is enabled, enable it.
1030 *
1031 * Disabling flickering when ae and awb are off allows a more precise
1032 * control of the programmed frame rate.
1033 */
1034 if (mt9v111->auto_exp->is_new || mt9v111->auto_awb->is_new) {
1035 if (mt9v111->auto_exp->val == V4L2_EXPOSURE_MANUAL &&
1036 mt9v111->auto_awb->val == V4L2_WHITE_BALANCE_MANUAL)
1037 ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
1038 MT9V111_IFP_R08_OUTFMT_CTRL,
1039 MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER,
1040 0);
1041 else
1042 ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
1043 MT9V111_IFP_R08_OUTFMT_CTRL,
1044 MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER,
1045 1);
1046 if (ret)
1047 return ret;
1048 }
1049
1050 ret = -EINVAL;
1051 switch (ctrl->id) {
1052 case V4L2_CID_AUTO_WHITE_BALANCE:
1053 ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
1054 MT9V111_IFP_R06_OPMODE_CTRL,
1055 MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN,
1056 ctrl->val == V4L2_WHITE_BALANCE_AUTO ?
1057 MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN : 0);
1058 break;
1059 case V4L2_CID_EXPOSURE_AUTO:
1060 ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP,
1061 MT9V111_IFP_R06_OPMODE_CTRL,
1062 MT9V111_IFP_R06_OPMODE_CTRL_AE_EN,
1063 ctrl->val == V4L2_EXPOSURE_AUTO ?
1064 MT9V111_IFP_R06_OPMODE_CTRL_AE_EN : 0);
1065 break;
1066 case V4L2_CID_HBLANK:
1067 ret = mt9v111_update(mt9v111->client, MT9V111_R01_CORE,
1068 MT9V111_CORE_R05_HBLANK,
1069 MT9V111_CORE_R05_MAX_HBLANK,
1070 mt9v111->hblank->val);
1071 break;
1072 case V4L2_CID_VBLANK:
1073 ret = mt9v111_update(mt9v111->client, MT9V111_R01_CORE,
1074 MT9V111_CORE_R06_VBLANK,
1075 MT9V111_CORE_R06_MAX_VBLANK,
1076 mt9v111->vblank->val);
1077 break;
1078 }
1079
1080 return ret;
1081 }
1082
1083 static const struct v4l2_ctrl_ops mt9v111_ctrl_ops = {
1084 .s_ctrl = mt9v111_s_ctrl,
1085 };
1086
mt9v111_chip_probe(struct mt9v111_dev * mt9v111)1087 static int mt9v111_chip_probe(struct mt9v111_dev *mt9v111)
1088 {
1089 int ret;
1090 u16 val;
1091
1092 ret = __mt9v111_power_on(&mt9v111->sd);
1093 if (ret)
1094 return ret;
1095
1096 ret = mt9v111_read(mt9v111->client, MT9V111_R01_CORE,
1097 MT9V111_CORE_RFF_CHIP_VER, &val);
1098 if (ret)
1099 goto power_off;
1100
1101 if ((val >> 8) != MT9V111_CHIP_ID_HIGH &&
1102 (val & 0xff) != MT9V111_CHIP_ID_LOW) {
1103 dev_err(mt9v111->dev,
1104 "Unable to identify MT9V111 chip: 0x%2x%2x\n",
1105 val >> 8, val & 0xff);
1106 ret = -EIO;
1107 goto power_off;
1108 }
1109
1110 dev_dbg(mt9v111->dev, "Chip identified: 0x%2x%2x\n",
1111 val >> 8, val & 0xff);
1112
1113 power_off:
1114 __mt9v111_power_off(&mt9v111->sd);
1115
1116 return ret;
1117 }
1118
mt9v111_probe(struct i2c_client * client)1119 static int mt9v111_probe(struct i2c_client *client)
1120 {
1121 struct mt9v111_dev *mt9v111;
1122 struct v4l2_fract tpf;
1123 int ret;
1124
1125 mt9v111 = devm_kzalloc(&client->dev, sizeof(*mt9v111), GFP_KERNEL);
1126 if (!mt9v111)
1127 return -ENOMEM;
1128
1129 mt9v111->dev = &client->dev;
1130 mt9v111->client = client;
1131
1132 mt9v111->clk = devm_clk_get(&client->dev, NULL);
1133 if (IS_ERR(mt9v111->clk))
1134 return PTR_ERR(mt9v111->clk);
1135
1136 mt9v111->sysclk = clk_get_rate(mt9v111->clk);
1137 if (mt9v111->sysclk > MT9V111_MAX_CLKIN)
1138 return -EINVAL;
1139
1140 mt9v111->oe = devm_gpiod_get_optional(&client->dev, "enable",
1141 GPIOD_OUT_LOW);
1142 if (IS_ERR(mt9v111->oe)) {
1143 dev_err(&client->dev, "Unable to get GPIO \"enable\": %ld\n",
1144 PTR_ERR(mt9v111->oe));
1145 return PTR_ERR(mt9v111->oe);
1146 }
1147
1148 mt9v111->standby = devm_gpiod_get_optional(&client->dev, "standby",
1149 GPIOD_OUT_HIGH);
1150 if (IS_ERR(mt9v111->standby)) {
1151 dev_err(&client->dev, "Unable to get GPIO \"standby\": %ld\n",
1152 PTR_ERR(mt9v111->standby));
1153 return PTR_ERR(mt9v111->standby);
1154 }
1155
1156 mt9v111->reset = devm_gpiod_get_optional(&client->dev, "reset",
1157 GPIOD_OUT_LOW);
1158 if (IS_ERR(mt9v111->reset)) {
1159 dev_err(&client->dev, "Unable to get GPIO \"reset\": %ld\n",
1160 PTR_ERR(mt9v111->reset));
1161 return PTR_ERR(mt9v111->reset);
1162 }
1163
1164 mutex_init(&mt9v111->pwr_mutex);
1165 mutex_init(&mt9v111->stream_mutex);
1166
1167 v4l2_ctrl_handler_init(&mt9v111->ctrls, 5);
1168
1169 mt9v111->auto_awb = v4l2_ctrl_new_std(&mt9v111->ctrls,
1170 &mt9v111_ctrl_ops,
1171 V4L2_CID_AUTO_WHITE_BALANCE,
1172 0, 1, 1,
1173 V4L2_WHITE_BALANCE_AUTO);
1174 mt9v111->auto_exp = v4l2_ctrl_new_std_menu(&mt9v111->ctrls,
1175 &mt9v111_ctrl_ops,
1176 V4L2_CID_EXPOSURE_AUTO,
1177 V4L2_EXPOSURE_MANUAL,
1178 0, V4L2_EXPOSURE_AUTO);
1179 mt9v111->hblank = v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops,
1180 V4L2_CID_HBLANK,
1181 MT9V111_CORE_R05_MIN_HBLANK,
1182 MT9V111_CORE_R05_MAX_HBLANK, 1,
1183 MT9V111_CORE_R05_DEF_HBLANK);
1184 mt9v111->vblank = v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops,
1185 V4L2_CID_VBLANK,
1186 MT9V111_CORE_R06_MIN_VBLANK,
1187 MT9V111_CORE_R06_MAX_VBLANK, 1,
1188 MT9V111_CORE_R06_DEF_VBLANK);
1189
1190 /* PIXEL_RATE is fixed: just expose it to user space. */
1191 v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops,
1192 V4L2_CID_PIXEL_RATE, 0,
1193 DIV_ROUND_CLOSEST(mt9v111->sysclk, 2), 1,
1194 DIV_ROUND_CLOSEST(mt9v111->sysclk, 2));
1195
1196 if (mt9v111->ctrls.error) {
1197 ret = mt9v111->ctrls.error;
1198 goto error_free_ctrls;
1199 }
1200 mt9v111->sd.ctrl_handler = &mt9v111->ctrls;
1201
1202 /* Start with default configuration: 640x480 UYVY. */
1203 mt9v111->fmt = mt9v111_def_fmt;
1204
1205 /* Re-calculate blankings for 640x480@15fps. */
1206 mt9v111->fps = 15;
1207 tpf.numerator = 1;
1208 tpf.denominator = mt9v111->fps;
1209 mt9v111_calc_frame_rate(mt9v111, &tpf);
1210
1211 mt9v111->pwr_count = 0;
1212 mt9v111->addr_space = MT9V111_R01_IFP;
1213 mt9v111->pending = true;
1214
1215 v4l2_i2c_subdev_init(&mt9v111->sd, client, &mt9v111_ops);
1216 mt9v111->sd.internal_ops = &mt9v111_internal_ops;
1217
1218 mt9v111->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1219 mt9v111->sd.entity.ops = &mt9v111_subdev_entity_ops;
1220 mt9v111->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1221
1222 mt9v111->pad.flags = MEDIA_PAD_FL_SOURCE;
1223 ret = media_entity_pads_init(&mt9v111->sd.entity, 1, &mt9v111->pad);
1224 if (ret)
1225 goto error_free_entity;
1226
1227 ret = mt9v111_chip_probe(mt9v111);
1228 if (ret)
1229 goto error_free_entity;
1230
1231 ret = v4l2_async_register_subdev(&mt9v111->sd);
1232 if (ret)
1233 goto error_free_entity;
1234
1235 return 0;
1236
1237 error_free_entity:
1238 media_entity_cleanup(&mt9v111->sd.entity);
1239
1240 error_free_ctrls:
1241 v4l2_ctrl_handler_free(&mt9v111->ctrls);
1242
1243 mutex_destroy(&mt9v111->pwr_mutex);
1244 mutex_destroy(&mt9v111->stream_mutex);
1245
1246 return ret;
1247 }
1248
mt9v111_remove(struct i2c_client * client)1249 static void mt9v111_remove(struct i2c_client *client)
1250 {
1251 struct v4l2_subdev *sd = i2c_get_clientdata(client);
1252 struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd);
1253
1254 v4l2_async_unregister_subdev(sd);
1255
1256 media_entity_cleanup(&sd->entity);
1257
1258 v4l2_ctrl_handler_free(&mt9v111->ctrls);
1259
1260 mutex_destroy(&mt9v111->pwr_mutex);
1261 mutex_destroy(&mt9v111->stream_mutex);
1262 }
1263
1264 static const struct of_device_id mt9v111_of_match[] = {
1265 { .compatible = "aptina,mt9v111", },
1266 { /* sentinel */ },
1267 };
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