Commit | Line | Data |
---|---|---|
aab7ed1c JM |
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 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 | ||
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 | ||
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 | ||
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 | ||
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 | ||
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 | ||
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 | ||
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 | ||
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 | ||
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 | ||
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 | ||
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 | ||
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 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 | ||
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 | ||
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 | ||
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 | ||
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 | ||
792 | static struct v4l2_mbus_framefmt *__mt9v111_get_pad_format( | |
793 | struct mt9v111_dev *mt9v111, | |
794 | struct v4l2_subdev_pad_config *cfg, | |
795 | unsigned int pad, | |
796 | enum v4l2_subdev_format_whence which) | |
797 | { | |
798 | switch (which) { | |
799 | case V4L2_SUBDEV_FORMAT_TRY: | |
484f9b37 | 800 | #if IS_ENABLED(CONFIG_VIDEO_V4L2_SUBDEV_API) |
aab7ed1c JM |
801 | return v4l2_subdev_get_try_format(&mt9v111->sd, cfg, pad); |
802 | #else | |
803 | return &cfg->try_fmt; | |
804 | #endif | |
805 | case V4L2_SUBDEV_FORMAT_ACTIVE: | |
806 | return &mt9v111->fmt; | |
807 | default: | |
808 | return NULL; | |
809 | } | |
810 | } | |
811 | ||
812 | static int mt9v111_enum_mbus_code(struct v4l2_subdev *subdev, | |
813 | struct v4l2_subdev_pad_config *cfg, | |
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 | ||
824 | static int mt9v111_enum_frame_interval(struct v4l2_subdev *sd, | |
825 | struct v4l2_subdev_pad_config *cfg, | |
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 | ||
847 | static int mt9v111_enum_frame_size(struct v4l2_subdev *subdev, | |
848 | struct v4l2_subdev_pad_config *cfg, | |
849 | struct v4l2_subdev_frame_size_enum *fse) | |
850 | { | |
d9a71866 | 851 | if (fse->pad || fse->index >= ARRAY_SIZE(mt9v111_frame_sizes)) |
aab7ed1c JM |
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 | ||
862 | static int mt9v111_get_format(struct v4l2_subdev *subdev, | |
863 | struct v4l2_subdev_pad_config *cfg, | |
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, cfg, format->pad, | |
873 | format->which); | |
874 | mutex_unlock(&mt9v111->stream_mutex); | |
875 | ||
876 | return 0; | |
877 | } | |
878 | ||
879 | static int mt9v111_set_format(struct v4l2_subdev *subdev, | |
880 | struct v4l2_subdev_pad_config *cfg, | |
881 | struct v4l2_subdev_format *format) | |
882 | { | |
883 | struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev); | |
884 | struct v4l2_mbus_framefmt new_fmt; | |
885 | struct v4l2_mbus_framefmt *__fmt; | |
886 | unsigned int best_fit = ~0L; | |
4effa8bf | 887 | unsigned int idx = 0; |
aab7ed1c JM |
888 | unsigned int i; |
889 | ||
890 | mutex_lock(&mt9v111->stream_mutex); | |
891 | if (mt9v111->streaming) { | |
892 | mutex_unlock(&mt9v111->stream_mutex); | |
893 | return -EBUSY; | |
894 | } | |
895 | ||
896 | if (format->pad) { | |
897 | mutex_unlock(&mt9v111->stream_mutex); | |
898 | return -EINVAL; | |
899 | } | |
900 | ||
901 | /* Update mbus format code and sizes. */ | |
902 | for (i = 0; i < ARRAY_SIZE(mt9v111_formats); i++) { | |
903 | if (format->format.code == mt9v111_formats[i].code) { | |
904 | new_fmt.code = mt9v111_formats[i].code; | |
905 | break; | |
906 | } | |
907 | } | |
908 | if (i == ARRAY_SIZE(mt9v111_formats)) | |
909 | new_fmt.code = mt9v111_formats[0].code; | |
910 | ||
911 | for (i = 0; i < ARRAY_SIZE(mt9v111_frame_sizes); i++) { | |
912 | unsigned int fit = abs(mt9v111_frame_sizes[i].width - | |
913 | format->format.width) + | |
914 | abs(mt9v111_frame_sizes[i].height - | |
915 | format->format.height); | |
916 | if (fit < best_fit) { | |
917 | best_fit = fit; | |
918 | idx = i; | |
919 | ||
920 | if (fit == 0) | |
921 | break; | |
922 | } | |
923 | } | |
924 | new_fmt.width = mt9v111_frame_sizes[idx].width; | |
925 | new_fmt.height = mt9v111_frame_sizes[idx].height; | |
926 | ||
927 | /* Update the device (or pad) format if it has changed. */ | |
928 | __fmt = __mt9v111_get_pad_format(mt9v111, cfg, format->pad, | |
929 | format->which); | |
930 | ||
931 | /* Format hasn't changed, stop here. */ | |
932 | if (__fmt->code == new_fmt.code && | |
933 | __fmt->width == new_fmt.width && | |
934 | __fmt->height == new_fmt.height) | |
935 | goto done; | |
936 | ||
937 | /* Update the format and sizes, then mark changes as pending. */ | |
938 | __fmt->code = new_fmt.code; | |
939 | __fmt->width = new_fmt.width; | |
940 | __fmt->height = new_fmt.height; | |
941 | ||
942 | if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE) | |
943 | mt9v111->pending = true; | |
944 | ||
945 | dev_dbg(mt9v111->dev, "%s: mbus_code: %x - (%ux%u)\n", | |
946 | __func__, __fmt->code, __fmt->width, __fmt->height); | |
947 | ||
948 | done: | |
949 | format->format = *__fmt; | |
950 | ||
951 | mutex_unlock(&mt9v111->stream_mutex); | |
952 | ||
953 | return 0; | |
954 | } | |
955 | ||
956 | static int mt9v111_init_cfg(struct v4l2_subdev *subdev, | |
957 | struct v4l2_subdev_pad_config *cfg) | |
958 | { | |
959 | cfg->try_fmt = mt9v111_def_fmt; | |
960 | ||
961 | return 0; | |
962 | } | |
963 | ||
964 | static const struct v4l2_subdev_core_ops mt9v111_core_ops = { | |
965 | .s_power = mt9v111_s_power, | |
966 | }; | |
967 | ||
968 | static const struct v4l2_subdev_video_ops mt9v111_video_ops = { | |
969 | .s_stream = mt9v111_s_stream, | |
970 | .s_frame_interval = mt9v111_s_frame_interval, | |
971 | .g_frame_interval = mt9v111_g_frame_interval, | |
972 | }; | |
973 | ||
974 | static const struct v4l2_subdev_pad_ops mt9v111_pad_ops = { | |
975 | .init_cfg = mt9v111_init_cfg, | |
976 | .enum_mbus_code = mt9v111_enum_mbus_code, | |
977 | .enum_frame_size = mt9v111_enum_frame_size, | |
978 | .enum_frame_interval = mt9v111_enum_frame_interval, | |
979 | .get_fmt = mt9v111_get_format, | |
980 | .set_fmt = mt9v111_set_format, | |
981 | }; | |
982 | ||
983 | static const struct v4l2_subdev_ops mt9v111_ops = { | |
984 | .core = &mt9v111_core_ops, | |
985 | .video = &mt9v111_video_ops, | |
986 | .pad = &mt9v111_pad_ops, | |
987 | }; | |
988 | ||
989 | #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER) | |
990 | static const struct media_entity_operations mt9v111_subdev_entity_ops = { | |
991 | .link_validate = v4l2_subdev_link_validate, | |
992 | }; | |
993 | #endif | |
994 | ||
995 | /* --- V4L2 ctrl --- */ | |
996 | static int mt9v111_s_ctrl(struct v4l2_ctrl *ctrl) | |
997 | { | |
998 | struct mt9v111_dev *mt9v111 = container_of(ctrl->handler, | |
999 | struct mt9v111_dev, | |
1000 | ctrls); | |
1001 | int ret; | |
1002 | ||
1003 | mutex_lock(&mt9v111->pwr_mutex); | |
1004 | /* | |
1005 | * If sensor is powered down, just cache new control values, | |
1006 | * no actual register access. | |
1007 | */ | |
1008 | if (!mt9v111->pwr_count) { | |
1009 | mt9v111->pending = true; | |
1010 | mutex_unlock(&mt9v111->pwr_mutex); | |
1011 | return 0; | |
1012 | } | |
1013 | mutex_unlock(&mt9v111->pwr_mutex); | |
1014 | ||
1015 | /* | |
1016 | * Flickering control gets disabled if both auto exp and auto awb | |
1017 | * are disabled too. If any of the two is enabled, enable it. | |
1018 | * | |
1019 | * Disabling flickering when ae and awb are off allows a more precise | |
1020 | * control of the programmed frame rate. | |
1021 | */ | |
1022 | if (mt9v111->auto_exp->is_new || mt9v111->auto_awb->is_new) { | |
1023 | if (mt9v111->auto_exp->val == V4L2_EXPOSURE_MANUAL && | |
1024 | mt9v111->auto_awb->val == V4L2_WHITE_BALANCE_MANUAL) | |
1025 | ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP, | |
1026 | MT9V111_IFP_R08_OUTFMT_CTRL, | |
1027 | MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER, | |
1028 | 0); | |
1029 | else | |
1030 | ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP, | |
1031 | MT9V111_IFP_R08_OUTFMT_CTRL, | |
1032 | MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER, | |
1033 | 1); | |
1034 | if (ret) | |
1035 | return ret; | |
1036 | } | |
1037 | ||
1038 | ret = -EINVAL; | |
1039 | switch (ctrl->id) { | |
1040 | case V4L2_CID_AUTO_WHITE_BALANCE: | |
1041 | ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP, | |
1042 | MT9V111_IFP_R06_OPMODE_CTRL, | |
1043 | MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN, | |
1044 | ctrl->val == V4L2_WHITE_BALANCE_AUTO ? | |
1045 | MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN : 0); | |
1046 | break; | |
1047 | case V4L2_CID_EXPOSURE_AUTO: | |
1048 | ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP, | |
1049 | MT9V111_IFP_R06_OPMODE_CTRL, | |
1050 | MT9V111_IFP_R06_OPMODE_CTRL_AE_EN, | |
1051 | ctrl->val == V4L2_EXPOSURE_AUTO ? | |
1052 | MT9V111_IFP_R06_OPMODE_CTRL_AE_EN : 0); | |
1053 | break; | |
1054 | case V4L2_CID_HBLANK: | |
1055 | ret = mt9v111_update(mt9v111->client, MT9V111_R01_CORE, | |
1056 | MT9V111_CORE_R05_HBLANK, | |
1057 | MT9V111_CORE_R05_MAX_HBLANK, | |
1058 | mt9v111->hblank->val); | |
1059 | break; | |
1060 | case V4L2_CID_VBLANK: | |
1061 | ret = mt9v111_update(mt9v111->client, MT9V111_R01_CORE, | |
1062 | MT9V111_CORE_R06_VBLANK, | |
1063 | MT9V111_CORE_R06_MAX_VBLANK, | |
1064 | mt9v111->vblank->val); | |
1065 | break; | |
1066 | } | |
1067 | ||
1068 | return ret; | |
1069 | } | |
1070 | ||
1071 | static const struct v4l2_ctrl_ops mt9v111_ctrl_ops = { | |
1072 | .s_ctrl = mt9v111_s_ctrl, | |
1073 | }; | |
1074 | ||
1075 | static int mt9v111_chip_probe(struct mt9v111_dev *mt9v111) | |
1076 | { | |
1077 | int ret; | |
1078 | u16 val; | |
1079 | ||
1080 | ret = __mt9v111_power_on(&mt9v111->sd); | |
1081 | if (ret) | |
1082 | return ret; | |
1083 | ||
1084 | ret = mt9v111_read(mt9v111->client, MT9V111_R01_CORE, | |
1085 | MT9V111_CORE_RFF_CHIP_VER, &val); | |
1086 | if (ret) | |
1087 | goto power_off; | |
1088 | ||
1089 | if ((val >> 8) != MT9V111_CHIP_ID_HIGH && | |
1090 | (val & 0xff) != MT9V111_CHIP_ID_LOW) { | |
1091 | dev_err(mt9v111->dev, | |
1092 | "Unable to identify MT9V111 chip: 0x%2x%2x\n", | |
1093 | val >> 8, val & 0xff); | |
1094 | ret = -EIO; | |
1095 | goto power_off; | |
1096 | } | |
1097 | ||
1098 | dev_dbg(mt9v111->dev, "Chip identified: 0x%2x%2x\n", | |
1099 | val >> 8, val & 0xff); | |
1100 | ||
1101 | power_off: | |
1102 | __mt9v111_power_off(&mt9v111->sd); | |
1103 | ||
1104 | return ret; | |
1105 | } | |
1106 | ||
1107 | static int mt9v111_probe(struct i2c_client *client) | |
1108 | { | |
1109 | struct mt9v111_dev *mt9v111; | |
1110 | struct v4l2_fract tpf; | |
1111 | int ret; | |
1112 | ||
1113 | mt9v111 = devm_kzalloc(&client->dev, sizeof(*mt9v111), GFP_KERNEL); | |
1114 | if (!mt9v111) | |
1115 | return -ENOMEM; | |
1116 | ||
1117 | mt9v111->dev = &client->dev; | |
1118 | mt9v111->client = client; | |
1119 | ||
1120 | mt9v111->clk = devm_clk_get(&client->dev, NULL); | |
1121 | if (IS_ERR(mt9v111->clk)) | |
1122 | return PTR_ERR(mt9v111->clk); | |
1123 | ||
1124 | mt9v111->sysclk = clk_get_rate(mt9v111->clk); | |
1125 | if (mt9v111->sysclk > MT9V111_MAX_CLKIN) | |
1126 | return -EINVAL; | |
1127 | ||
1128 | mt9v111->oe = devm_gpiod_get_optional(&client->dev, "enable", | |
1129 | GPIOD_OUT_LOW); | |
1130 | if (IS_ERR(mt9v111->oe)) { | |
1131 | dev_err(&client->dev, "Unable to get GPIO \"enable\": %ld\n", | |
1132 | PTR_ERR(mt9v111->oe)); | |
1133 | return PTR_ERR(mt9v111->oe); | |
1134 | } | |
1135 | ||
1136 | mt9v111->standby = devm_gpiod_get_optional(&client->dev, "standby", | |
1137 | GPIOD_OUT_HIGH); | |
1138 | if (IS_ERR(mt9v111->standby)) { | |
1139 | dev_err(&client->dev, "Unable to get GPIO \"standby\": %ld\n", | |
1140 | PTR_ERR(mt9v111->standby)); | |
1141 | return PTR_ERR(mt9v111->standby); | |
1142 | } | |
1143 | ||
1144 | mt9v111->reset = devm_gpiod_get_optional(&client->dev, "reset", | |
1145 | GPIOD_OUT_LOW); | |
1146 | if (IS_ERR(mt9v111->reset)) { | |
1147 | dev_err(&client->dev, "Unable to get GPIO \"reset\": %ld\n", | |
1148 | PTR_ERR(mt9v111->reset)); | |
1149 | return PTR_ERR(mt9v111->reset); | |
1150 | } | |
1151 | ||
1152 | mutex_init(&mt9v111->pwr_mutex); | |
1153 | mutex_init(&mt9v111->stream_mutex); | |
1154 | ||
1155 | v4l2_ctrl_handler_init(&mt9v111->ctrls, 5); | |
1156 | ||
1157 | mt9v111->auto_awb = v4l2_ctrl_new_std(&mt9v111->ctrls, | |
1158 | &mt9v111_ctrl_ops, | |
1159 | V4L2_CID_AUTO_WHITE_BALANCE, | |
1160 | 0, 1, 1, | |
1161 | V4L2_WHITE_BALANCE_AUTO); | |
aab7ed1c JM |
1162 | mt9v111->auto_exp = v4l2_ctrl_new_std_menu(&mt9v111->ctrls, |
1163 | &mt9v111_ctrl_ops, | |
1164 | V4L2_CID_EXPOSURE_AUTO, | |
1165 | V4L2_EXPOSURE_MANUAL, | |
1166 | 0, V4L2_EXPOSURE_AUTO); | |
aab7ed1c JM |
1167 | mt9v111->hblank = v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops, |
1168 | V4L2_CID_HBLANK, | |
1169 | MT9V111_CORE_R05_MIN_HBLANK, | |
1170 | MT9V111_CORE_R05_MAX_HBLANK, 1, | |
1171 | MT9V111_CORE_R05_DEF_HBLANK); | |
aab7ed1c JM |
1172 | mt9v111->vblank = v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops, |
1173 | V4L2_CID_VBLANK, | |
1174 | MT9V111_CORE_R06_MIN_VBLANK, | |
1175 | MT9V111_CORE_R06_MAX_VBLANK, 1, | |
1176 | MT9V111_CORE_R06_DEF_VBLANK); | |
aab7ed1c JM |
1177 | |
1178 | /* PIXEL_RATE is fixed: just expose it to user space. */ | |
1179 | v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops, | |
1180 | V4L2_CID_PIXEL_RATE, 0, | |
1181 | DIV_ROUND_CLOSEST(mt9v111->sysclk, 2), 1, | |
1182 | DIV_ROUND_CLOSEST(mt9v111->sysclk, 2)); | |
1183 | ||
9d2ac5f4 JM |
1184 | if (mt9v111->ctrls.error) { |
1185 | ret = mt9v111->ctrls.error; | |
1186 | goto error_free_ctrls; | |
1187 | } | |
aab7ed1c JM |
1188 | mt9v111->sd.ctrl_handler = &mt9v111->ctrls; |
1189 | ||
1190 | /* Start with default configuration: 640x480 UYVY. */ | |
1191 | mt9v111->fmt = mt9v111_def_fmt; | |
1192 | ||
1193 | /* Re-calculate blankings for 640x480@15fps. */ | |
1194 | mt9v111->fps = 15; | |
1195 | tpf.numerator = 1; | |
1196 | tpf.denominator = mt9v111->fps; | |
1197 | mt9v111_calc_frame_rate(mt9v111, &tpf); | |
1198 | ||
1199 | mt9v111->pwr_count = 0; | |
1200 | mt9v111->addr_space = MT9V111_R01_IFP; | |
1201 | mt9v111->pending = true; | |
1202 | ||
1203 | v4l2_i2c_subdev_init(&mt9v111->sd, client, &mt9v111_ops); | |
1204 | ||
1205 | #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER) | |
1206 | mt9v111->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE; | |
1207 | mt9v111->sd.entity.ops = &mt9v111_subdev_entity_ops; | |
1208 | mt9v111->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR; | |
1209 | ||
1210 | mt9v111->pad.flags = MEDIA_PAD_FL_SOURCE; | |
1211 | ret = media_entity_pads_init(&mt9v111->sd.entity, 1, &mt9v111->pad); | |
1212 | if (ret) | |
9d2ac5f4 | 1213 | goto error_free_entity; |
aab7ed1c JM |
1214 | #endif |
1215 | ||
1216 | ret = mt9v111_chip_probe(mt9v111); | |
1217 | if (ret) | |
9d2ac5f4 | 1218 | goto error_free_entity; |
aab7ed1c JM |
1219 | |
1220 | ret = v4l2_async_register_subdev(&mt9v111->sd); | |
1221 | if (ret) | |
9d2ac5f4 | 1222 | goto error_free_entity; |
aab7ed1c JM |
1223 | |
1224 | return 0; | |
1225 | ||
9d2ac5f4 | 1226 | error_free_entity: |
aab7ed1c JM |
1227 | #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER) |
1228 | media_entity_cleanup(&mt9v111->sd.entity); | |
1229 | #endif | |
1230 | ||
9d2ac5f4 JM |
1231 | error_free_ctrls: |
1232 | v4l2_ctrl_handler_free(&mt9v111->ctrls); | |
1233 | ||
aab7ed1c JM |
1234 | mutex_destroy(&mt9v111->pwr_mutex); |
1235 | mutex_destroy(&mt9v111->stream_mutex); | |
1236 | ||
1237 | return ret; | |
1238 | } | |
1239 | ||
1240 | static int mt9v111_remove(struct i2c_client *client) | |
1241 | { | |
1242 | struct v4l2_subdev *sd = i2c_get_clientdata(client); | |
1243 | struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd); | |
1244 | ||
1245 | v4l2_async_unregister_subdev(sd); | |
1246 | ||
aab7ed1c JM |
1247 | #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER) |
1248 | media_entity_cleanup(&sd->entity); | |
1249 | #endif | |
1250 | ||
9d2ac5f4 JM |
1251 | v4l2_ctrl_handler_free(&mt9v111->ctrls); |
1252 | ||
aab7ed1c JM |
1253 | mutex_destroy(&mt9v111->pwr_mutex); |
1254 | mutex_destroy(&mt9v111->stream_mutex); | |
1255 | ||
1256 | devm_gpiod_put(mt9v111->dev, mt9v111->oe); | |
1257 | devm_gpiod_put(mt9v111->dev, mt9v111->standby); | |
1258 | devm_gpiod_put(mt9v111->dev, mt9v111->reset); | |
1259 | ||
1260 | devm_clk_put(mt9v111->dev, mt9v111->clk); | |
1261 | ||
1262 | return 0; | |
1263 | } | |
1264 | ||
1265 | static const struct of_device_id mt9v111_of_match[] = { | |
1266 | { .compatible = "aptina,mt9v111", }, | |
1267 | { /* sentinel */ }, | |
1268 | }; | |
1269 | ||
1270 | static struct i2c_driver mt9v111_driver = { | |
1271 | .driver = { | |
1272 | .name = "mt9v111", | |
1273 | .of_match_table = mt9v111_of_match, | |
1274 | }, | |
1275 | .probe_new = mt9v111_probe, | |
1276 | .remove = mt9v111_remove, | |
1277 | }; | |
1278 | ||
1279 | module_i2c_driver(mt9v111_driver); | |
1280 | ||
1281 | MODULE_DESCRIPTION("V4L2 sensor driver for Aptina MT9V111"); | |
1282 | MODULE_AUTHOR("Jacopo Mondi <jacopo@jmondi.org>"); | |
1283 | MODULE_LICENSE("GPL v2"); |