1 // SPDX-License-Identifier: GPL-2.0 2 /* 3 * V4L2 sensor driver for Aptina MT9V111 image sensor 4 * Copyright (C) 2018 Jacopo Mondi <jacopo@jmondi.org> 5 * 6 * Based on mt9v032 driver 7 * Copyright (C) 2010, Laurent Pinchart <laurent.pinchart@ideasonboard.com> 8 * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de> 9 * 10 * Based on mt9v011 driver 11 * Copyright (c) 2009 Mauro Carvalho Chehab <mchehab@kernel.org> 12 */ 13 14 #include <linux/clk.h> 15 #include <linux/delay.h> 16 #include <linux/gpio/consumer.h> 17 #include <linux/i2c.h> 18 #include <linux/of.h> 19 #include <linux/slab.h> 20 #include <linux/videodev2.h> 21 #include <linux/v4l2-mediabus.h> 22 #include <linux/module.h> 23 24 #include <media/v4l2-ctrls.h> 25 #include <media/v4l2-device.h> 26 #include <media/v4l2-fwnode.h> 27 #include <media/v4l2-image-sizes.h> 28 #include <media/v4l2-subdev.h> 29 30 /* 31 * MT9V111 is a 1/4-Inch CMOS digital image sensor with an integrated 32 * Image Flow Processing (IFP) engine and a sensor core loosely based on 33 * MT9V011. 34 * 35 * The IFP can produce several output image formats from the sensor core 36 * output. This driver currently supports only YUYV format permutations. 37 * 38 * The driver allows manual frame rate control through s_frame_interval subdev 39 * operation or V4L2_CID_V/HBLANK controls, but it is known that the 40 * auto-exposure algorithm might modify the programmed frame rate. While the 41 * driver initially programs the sensor with auto-exposure and 42 * auto-white-balancing enabled, it is possible to disable them and more 43 * precisely control the frame rate. 44 * 45 * While it seems possible to instruct the auto-exposure control algorithm to 46 * respect a programmed frame rate when adjusting the pixel integration time, 47 * registers controlling this feature are not documented in the public 48 * available sensor manual used to develop this driver (09005aef80e90084, 49 * MT9V111_1.fm - Rev. G 1/05 EN). 50 */ 51 52 #define MT9V111_CHIP_ID_HIGH 0x82 53 #define MT9V111_CHIP_ID_LOW 0x3a 54 55 #define MT9V111_R01_ADDR_SPACE 0x01 56 #define MT9V111_R01_IFP 0x01 57 #define MT9V111_R01_CORE 0x04 58 59 #define MT9V111_IFP_R06_OPMODE_CTRL 0x06 60 #define MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN BIT(1) 61 #define MT9V111_IFP_R06_OPMODE_CTRL_AE_EN BIT(14) 62 #define MT9V111_IFP_R07_IFP_RESET 0x07 63 #define MT9V111_IFP_R07_IFP_RESET_MASK BIT(0) 64 #define MT9V111_IFP_R08_OUTFMT_CTRL 0x08 65 #define MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER BIT(11) 66 #define MT9V111_IFP_R08_OUTFMT_CTRL_PCLK BIT(5) 67 #define MT9V111_IFP_R3A_OUTFMT_CTRL2 0x3a 68 #define MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_CBCR BIT(0) 69 #define MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_YC BIT(1) 70 #define MT9V111_IFP_R3A_OUTFMT_CTRL2_SWAP_MASK GENMASK(2, 0) 71 #define MT9V111_IFP_RA5_HPAN 0xa5 72 #define MT9V111_IFP_RA6_HZOOM 0xa6 73 #define MT9V111_IFP_RA7_HOUT 0xa7 74 #define MT9V111_IFP_RA8_VPAN 0xa8 75 #define MT9V111_IFP_RA9_VZOOM 0xa9 76 #define MT9V111_IFP_RAA_VOUT 0xaa 77 #define MT9V111_IFP_DECIMATION_MASK GENMASK(9, 0) 78 #define MT9V111_IFP_DECIMATION_FREEZE BIT(15) 79 80 #define MT9V111_CORE_R03_WIN_HEIGHT 0x03 81 #define MT9V111_CORE_R03_WIN_V_OFFS 2 82 #define MT9V111_CORE_R04_WIN_WIDTH 0x04 83 #define MT9V111_CORE_R04_WIN_H_OFFS 114 84 #define MT9V111_CORE_R05_HBLANK 0x05 85 #define MT9V111_CORE_R05_MIN_HBLANK 0x09 86 #define MT9V111_CORE_R05_MAX_HBLANK GENMASK(9, 0) 87 #define MT9V111_CORE_R05_DEF_HBLANK 0x26 88 #define MT9V111_CORE_R06_VBLANK 0x06 89 #define MT9V111_CORE_R06_MIN_VBLANK 0x03 90 #define MT9V111_CORE_R06_MAX_VBLANK GENMASK(11, 0) 91 #define MT9V111_CORE_R06_DEF_VBLANK 0x04 92 #define MT9V111_CORE_R07_OUT_CTRL 0x07 93 #define MT9V111_CORE_R07_OUT_CTRL_SAMPLE BIT(4) 94 #define MT9V111_CORE_R09_PIXEL_INT 0x09 95 #define MT9V111_CORE_R09_PIXEL_INT_MASK GENMASK(11, 0) 96 #define MT9V111_CORE_R0D_CORE_RESET 0x0d 97 #define MT9V111_CORE_R0D_CORE_RESET_MASK BIT(0) 98 #define MT9V111_CORE_RFF_CHIP_VER 0xff 99 100 #define MT9V111_PIXEL_ARRAY_WIDTH 640 101 #define MT9V111_PIXEL_ARRAY_HEIGHT 480 102 103 #define MT9V111_MAX_CLKIN 27000000 104 105 /* The default sensor configuration at startup time. */ 106 static const struct v4l2_mbus_framefmt mt9v111_def_fmt = { 107 .width = 640, 108 .height = 480, 109 .code = MEDIA_BUS_FMT_UYVY8_2X8, 110 .field = V4L2_FIELD_NONE, 111 .colorspace = V4L2_COLORSPACE_SRGB, 112 .ycbcr_enc = V4L2_YCBCR_ENC_601, 113 .quantization = V4L2_QUANTIZATION_LIM_RANGE, 114 .xfer_func = V4L2_XFER_FUNC_SRGB, 115 }; 116 117 struct mt9v111_dev { 118 struct device *dev; 119 struct i2c_client *client; 120 121 u8 addr_space; 122 123 struct v4l2_subdev sd; 124 #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER) 125 struct media_pad pad; 126 #endif 127 128 struct v4l2_ctrl *auto_awb; 129 struct v4l2_ctrl *auto_exp; 130 struct v4l2_ctrl *hblank; 131 struct v4l2_ctrl *vblank; 132 struct v4l2_ctrl_handler ctrls; 133 134 /* Output image format and sizes. */ 135 struct v4l2_mbus_framefmt fmt; 136 unsigned int fps; 137 138 /* Protects power up/down sequences. */ 139 struct mutex pwr_mutex; 140 int pwr_count; 141 142 /* Protects stream on/off sequences. */ 143 struct mutex stream_mutex; 144 bool streaming; 145 146 /* Flags to mark HW settings as not yet applied. */ 147 bool pending; 148 149 /* Clock provider and system clock frequency. */ 150 struct clk *clk; 151 u32 sysclk; 152 153 struct gpio_desc *oe; 154 struct gpio_desc *standby; 155 struct gpio_desc *reset; 156 }; 157 158 #define sd_to_mt9v111(__sd) container_of((__sd), struct mt9v111_dev, sd) 159 160 /* 161 * mt9v111_mbus_fmt - List all media bus formats supported by the driver. 162 * 163 * Only list the media bus code here. The image sizes are freely configurable 164 * in the pixel array sizes range. 165 * 166 * The desired frame interval, in the supported frame interval range, is 167 * obtained by configuring blanking as the sensor does not have a PLL but 168 * only a fixed clock divider that generates the output pixel clock. 169 */ 170 static struct mt9v111_mbus_fmt { 171 u32 code; 172 } mt9v111_formats[] = { 173 { 174 .code = MEDIA_BUS_FMT_UYVY8_2X8, 175 }, 176 { 177 .code = MEDIA_BUS_FMT_YUYV8_2X8, 178 }, 179 { 180 .code = MEDIA_BUS_FMT_VYUY8_2X8, 181 }, 182 { 183 .code = MEDIA_BUS_FMT_YVYU8_2X8, 184 }, 185 }; 186 187 static u32 mt9v111_frame_intervals[] = {5, 10, 15, 20, 30}; 188 189 /* 190 * mt9v111_frame_sizes - List sensor's supported resolutions. 191 * 192 * Resolution generated through decimation in the IFP block from the 193 * full VGA pixel array. 194 */ 195 static struct v4l2_rect mt9v111_frame_sizes[] = { 196 { 197 .width = 640, 198 .height = 480, 199 }, 200 { 201 .width = 352, 202 .height = 288 203 }, 204 { 205 .width = 320, 206 .height = 240, 207 }, 208 { 209 .width = 176, 210 .height = 144, 211 }, 212 { 213 .width = 160, 214 .height = 120, 215 }, 216 }; 217 218 /* --- Device I/O access --- */ 219 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 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_state *sd_state, 795 unsigned int pad, 796 enum v4l2_subdev_format_whence which) 797 { 798 switch (which) { 799 case V4L2_SUBDEV_FORMAT_TRY: 800 #if IS_ENABLED(CONFIG_VIDEO_V4L2_SUBDEV_API) 801 return v4l2_subdev_get_try_format(&mt9v111->sd, sd_state, pad); 802 #else 803 return &sd_state->pads->try_fmt; 804 #endif 805 case V4L2_SUBDEV_FORMAT_ACTIVE: 806 return &mt9v111->fmt; 807 default: 808 return NULL; 809 } 810 } 811 812 static int mt9v111_enum_mbus_code(struct v4l2_subdev *subdev, 813 struct v4l2_subdev_state *sd_state, 814 struct v4l2_subdev_mbus_code_enum *code) 815 { 816 if (code->pad || code->index > ARRAY_SIZE(mt9v111_formats) - 1) 817 return -EINVAL; 818 819 code->code = mt9v111_formats[code->index].code; 820 821 return 0; 822 } 823 824 static int mt9v111_enum_frame_interval(struct v4l2_subdev *sd, 825 struct v4l2_subdev_state *sd_state, 826 struct v4l2_subdev_frame_interval_enum *fie) 827 { 828 unsigned int i; 829 830 if (fie->pad || fie->index >= ARRAY_SIZE(mt9v111_frame_intervals)) 831 return -EINVAL; 832 833 for (i = 0; i < ARRAY_SIZE(mt9v111_frame_sizes); i++) 834 if (fie->width == mt9v111_frame_sizes[i].width && 835 fie->height == mt9v111_frame_sizes[i].height) 836 break; 837 838 if (i == ARRAY_SIZE(mt9v111_frame_sizes)) 839 return -EINVAL; 840 841 fie->interval.numerator = 1; 842 fie->interval.denominator = mt9v111_frame_intervals[fie->index]; 843 844 return 0; 845 } 846 847 static int mt9v111_enum_frame_size(struct v4l2_subdev *subdev, 848 struct v4l2_subdev_state *sd_state, 849 struct v4l2_subdev_frame_size_enum *fse) 850 { 851 if (fse->pad || fse->index >= ARRAY_SIZE(mt9v111_frame_sizes)) 852 return -EINVAL; 853 854 fse->min_width = mt9v111_frame_sizes[fse->index].width; 855 fse->max_width = mt9v111_frame_sizes[fse->index].width; 856 fse->min_height = mt9v111_frame_sizes[fse->index].height; 857 fse->max_height = mt9v111_frame_sizes[fse->index].height; 858 859 return 0; 860 } 861 862 static int mt9v111_get_format(struct v4l2_subdev *subdev, 863 struct v4l2_subdev_state *sd_state, 864 struct v4l2_subdev_format *format) 865 { 866 struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev); 867 868 if (format->pad) 869 return -EINVAL; 870 871 mutex_lock(&mt9v111->stream_mutex); 872 format->format = *__mt9v111_get_pad_format(mt9v111, sd_state, 873 format->pad, 874 format->which); 875 mutex_unlock(&mt9v111->stream_mutex); 876 877 return 0; 878 } 879 880 static int mt9v111_set_format(struct v4l2_subdev *subdev, 881 struct v4l2_subdev_state *sd_state, 882 struct v4l2_subdev_format *format) 883 { 884 struct mt9v111_dev *mt9v111 = sd_to_mt9v111(subdev); 885 struct v4l2_mbus_framefmt new_fmt; 886 struct v4l2_mbus_framefmt *__fmt; 887 unsigned int best_fit = ~0L; 888 unsigned int idx = 0; 889 unsigned int i; 890 891 mutex_lock(&mt9v111->stream_mutex); 892 if (mt9v111->streaming) { 893 mutex_unlock(&mt9v111->stream_mutex); 894 return -EBUSY; 895 } 896 897 if (format->pad) { 898 mutex_unlock(&mt9v111->stream_mutex); 899 return -EINVAL; 900 } 901 902 /* Update mbus format code and sizes. */ 903 for (i = 0; i < ARRAY_SIZE(mt9v111_formats); i++) { 904 if (format->format.code == mt9v111_formats[i].code) { 905 new_fmt.code = mt9v111_formats[i].code; 906 break; 907 } 908 } 909 if (i == ARRAY_SIZE(mt9v111_formats)) 910 new_fmt.code = mt9v111_formats[0].code; 911 912 for (i = 0; i < ARRAY_SIZE(mt9v111_frame_sizes); i++) { 913 unsigned int fit = abs(mt9v111_frame_sizes[i].width - 914 format->format.width) + 915 abs(mt9v111_frame_sizes[i].height - 916 format->format.height); 917 if (fit < best_fit) { 918 best_fit = fit; 919 idx = i; 920 921 if (fit == 0) 922 break; 923 } 924 } 925 new_fmt.width = mt9v111_frame_sizes[idx].width; 926 new_fmt.height = mt9v111_frame_sizes[idx].height; 927 928 /* Update the device (or pad) format if it has changed. */ 929 __fmt = __mt9v111_get_pad_format(mt9v111, sd_state, format->pad, 930 format->which); 931 932 /* Format hasn't changed, stop here. */ 933 if (__fmt->code == new_fmt.code && 934 __fmt->width == new_fmt.width && 935 __fmt->height == new_fmt.height) 936 goto done; 937 938 /* Update the format and sizes, then mark changes as pending. */ 939 __fmt->code = new_fmt.code; 940 __fmt->width = new_fmt.width; 941 __fmt->height = new_fmt.height; 942 943 if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE) 944 mt9v111->pending = true; 945 946 dev_dbg(mt9v111->dev, "%s: mbus_code: %x - (%ux%u)\n", 947 __func__, __fmt->code, __fmt->width, __fmt->height); 948 949 done: 950 format->format = *__fmt; 951 952 mutex_unlock(&mt9v111->stream_mutex); 953 954 return 0; 955 } 956 957 static int mt9v111_init_cfg(struct v4l2_subdev *subdev, 958 struct v4l2_subdev_state *sd_state) 959 { 960 sd_state->pads->try_fmt = mt9v111_def_fmt; 961 962 return 0; 963 } 964 965 static const struct v4l2_subdev_core_ops mt9v111_core_ops = { 966 .s_power = mt9v111_s_power, 967 }; 968 969 static const struct v4l2_subdev_video_ops mt9v111_video_ops = { 970 .s_stream = mt9v111_s_stream, 971 .s_frame_interval = mt9v111_s_frame_interval, 972 .g_frame_interval = mt9v111_g_frame_interval, 973 }; 974 975 static const struct v4l2_subdev_pad_ops mt9v111_pad_ops = { 976 .init_cfg = mt9v111_init_cfg, 977 .enum_mbus_code = mt9v111_enum_mbus_code, 978 .enum_frame_size = mt9v111_enum_frame_size, 979 .enum_frame_interval = mt9v111_enum_frame_interval, 980 .get_fmt = mt9v111_get_format, 981 .set_fmt = mt9v111_set_format, 982 }; 983 984 static const struct v4l2_subdev_ops mt9v111_ops = { 985 .core = &mt9v111_core_ops, 986 .video = &mt9v111_video_ops, 987 .pad = &mt9v111_pad_ops, 988 }; 989 990 #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER) 991 static const struct media_entity_operations mt9v111_subdev_entity_ops = { 992 .link_validate = v4l2_subdev_link_validate, 993 }; 994 #endif 995 996 /* --- V4L2 ctrl --- */ 997 static int mt9v111_s_ctrl(struct v4l2_ctrl *ctrl) 998 { 999 struct mt9v111_dev *mt9v111 = container_of(ctrl->handler, 1000 struct mt9v111_dev, 1001 ctrls); 1002 int ret; 1003 1004 mutex_lock(&mt9v111->pwr_mutex); 1005 /* 1006 * If sensor is powered down, just cache new control values, 1007 * no actual register access. 1008 */ 1009 if (!mt9v111->pwr_count) { 1010 mt9v111->pending = true; 1011 mutex_unlock(&mt9v111->pwr_mutex); 1012 return 0; 1013 } 1014 mutex_unlock(&mt9v111->pwr_mutex); 1015 1016 /* 1017 * Flickering control gets disabled if both auto exp and auto awb 1018 * are disabled too. If any of the two is enabled, enable it. 1019 * 1020 * Disabling flickering when ae and awb are off allows a more precise 1021 * control of the programmed frame rate. 1022 */ 1023 if (mt9v111->auto_exp->is_new || mt9v111->auto_awb->is_new) { 1024 if (mt9v111->auto_exp->val == V4L2_EXPOSURE_MANUAL && 1025 mt9v111->auto_awb->val == V4L2_WHITE_BALANCE_MANUAL) 1026 ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP, 1027 MT9V111_IFP_R08_OUTFMT_CTRL, 1028 MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER, 1029 0); 1030 else 1031 ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP, 1032 MT9V111_IFP_R08_OUTFMT_CTRL, 1033 MT9V111_IFP_R08_OUTFMT_CTRL_FLICKER, 1034 1); 1035 if (ret) 1036 return ret; 1037 } 1038 1039 ret = -EINVAL; 1040 switch (ctrl->id) { 1041 case V4L2_CID_AUTO_WHITE_BALANCE: 1042 ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP, 1043 MT9V111_IFP_R06_OPMODE_CTRL, 1044 MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN, 1045 ctrl->val == V4L2_WHITE_BALANCE_AUTO ? 1046 MT9V111_IFP_R06_OPMODE_CTRL_AWB_EN : 0); 1047 break; 1048 case V4L2_CID_EXPOSURE_AUTO: 1049 ret = mt9v111_update(mt9v111->client, MT9V111_R01_IFP, 1050 MT9V111_IFP_R06_OPMODE_CTRL, 1051 MT9V111_IFP_R06_OPMODE_CTRL_AE_EN, 1052 ctrl->val == V4L2_EXPOSURE_AUTO ? 1053 MT9V111_IFP_R06_OPMODE_CTRL_AE_EN : 0); 1054 break; 1055 case V4L2_CID_HBLANK: 1056 ret = mt9v111_update(mt9v111->client, MT9V111_R01_CORE, 1057 MT9V111_CORE_R05_HBLANK, 1058 MT9V111_CORE_R05_MAX_HBLANK, 1059 mt9v111->hblank->val); 1060 break; 1061 case V4L2_CID_VBLANK: 1062 ret = mt9v111_update(mt9v111->client, MT9V111_R01_CORE, 1063 MT9V111_CORE_R06_VBLANK, 1064 MT9V111_CORE_R06_MAX_VBLANK, 1065 mt9v111->vblank->val); 1066 break; 1067 } 1068 1069 return ret; 1070 } 1071 1072 static const struct v4l2_ctrl_ops mt9v111_ctrl_ops = { 1073 .s_ctrl = mt9v111_s_ctrl, 1074 }; 1075 1076 static int mt9v111_chip_probe(struct mt9v111_dev *mt9v111) 1077 { 1078 int ret; 1079 u16 val; 1080 1081 ret = __mt9v111_power_on(&mt9v111->sd); 1082 if (ret) 1083 return ret; 1084 1085 ret = mt9v111_read(mt9v111->client, MT9V111_R01_CORE, 1086 MT9V111_CORE_RFF_CHIP_VER, &val); 1087 if (ret) 1088 goto power_off; 1089 1090 if ((val >> 8) != MT9V111_CHIP_ID_HIGH && 1091 (val & 0xff) != MT9V111_CHIP_ID_LOW) { 1092 dev_err(mt9v111->dev, 1093 "Unable to identify MT9V111 chip: 0x%2x%2x\n", 1094 val >> 8, val & 0xff); 1095 ret = -EIO; 1096 goto power_off; 1097 } 1098 1099 dev_dbg(mt9v111->dev, "Chip identified: 0x%2x%2x\n", 1100 val >> 8, val & 0xff); 1101 1102 power_off: 1103 __mt9v111_power_off(&mt9v111->sd); 1104 1105 return ret; 1106 } 1107 1108 static int mt9v111_probe(struct i2c_client *client) 1109 { 1110 struct mt9v111_dev *mt9v111; 1111 struct v4l2_fract tpf; 1112 int ret; 1113 1114 mt9v111 = devm_kzalloc(&client->dev, sizeof(*mt9v111), GFP_KERNEL); 1115 if (!mt9v111) 1116 return -ENOMEM; 1117 1118 mt9v111->dev = &client->dev; 1119 mt9v111->client = client; 1120 1121 mt9v111->clk = devm_clk_get(&client->dev, NULL); 1122 if (IS_ERR(mt9v111->clk)) 1123 return PTR_ERR(mt9v111->clk); 1124 1125 mt9v111->sysclk = clk_get_rate(mt9v111->clk); 1126 if (mt9v111->sysclk > MT9V111_MAX_CLKIN) 1127 return -EINVAL; 1128 1129 mt9v111->oe = devm_gpiod_get_optional(&client->dev, "enable", 1130 GPIOD_OUT_LOW); 1131 if (IS_ERR(mt9v111->oe)) { 1132 dev_err(&client->dev, "Unable to get GPIO \"enable\": %ld\n", 1133 PTR_ERR(mt9v111->oe)); 1134 return PTR_ERR(mt9v111->oe); 1135 } 1136 1137 mt9v111->standby = devm_gpiod_get_optional(&client->dev, "standby", 1138 GPIOD_OUT_HIGH); 1139 if (IS_ERR(mt9v111->standby)) { 1140 dev_err(&client->dev, "Unable to get GPIO \"standby\": %ld\n", 1141 PTR_ERR(mt9v111->standby)); 1142 return PTR_ERR(mt9v111->standby); 1143 } 1144 1145 mt9v111->reset = devm_gpiod_get_optional(&client->dev, "reset", 1146 GPIOD_OUT_LOW); 1147 if (IS_ERR(mt9v111->reset)) { 1148 dev_err(&client->dev, "Unable to get GPIO \"reset\": %ld\n", 1149 PTR_ERR(mt9v111->reset)); 1150 return PTR_ERR(mt9v111->reset); 1151 } 1152 1153 mutex_init(&mt9v111->pwr_mutex); 1154 mutex_init(&mt9v111->stream_mutex); 1155 1156 v4l2_ctrl_handler_init(&mt9v111->ctrls, 5); 1157 1158 mt9v111->auto_awb = v4l2_ctrl_new_std(&mt9v111->ctrls, 1159 &mt9v111_ctrl_ops, 1160 V4L2_CID_AUTO_WHITE_BALANCE, 1161 0, 1, 1, 1162 V4L2_WHITE_BALANCE_AUTO); 1163 mt9v111->auto_exp = v4l2_ctrl_new_std_menu(&mt9v111->ctrls, 1164 &mt9v111_ctrl_ops, 1165 V4L2_CID_EXPOSURE_AUTO, 1166 V4L2_EXPOSURE_MANUAL, 1167 0, V4L2_EXPOSURE_AUTO); 1168 mt9v111->hblank = v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops, 1169 V4L2_CID_HBLANK, 1170 MT9V111_CORE_R05_MIN_HBLANK, 1171 MT9V111_CORE_R05_MAX_HBLANK, 1, 1172 MT9V111_CORE_R05_DEF_HBLANK); 1173 mt9v111->vblank = v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops, 1174 V4L2_CID_VBLANK, 1175 MT9V111_CORE_R06_MIN_VBLANK, 1176 MT9V111_CORE_R06_MAX_VBLANK, 1, 1177 MT9V111_CORE_R06_DEF_VBLANK); 1178 1179 /* PIXEL_RATE is fixed: just expose it to user space. */ 1180 v4l2_ctrl_new_std(&mt9v111->ctrls, &mt9v111_ctrl_ops, 1181 V4L2_CID_PIXEL_RATE, 0, 1182 DIV_ROUND_CLOSEST(mt9v111->sysclk, 2), 1, 1183 DIV_ROUND_CLOSEST(mt9v111->sysclk, 2)); 1184 1185 if (mt9v111->ctrls.error) { 1186 ret = mt9v111->ctrls.error; 1187 goto error_free_ctrls; 1188 } 1189 mt9v111->sd.ctrl_handler = &mt9v111->ctrls; 1190 1191 /* Start with default configuration: 640x480 UYVY. */ 1192 mt9v111->fmt = mt9v111_def_fmt; 1193 1194 /* Re-calculate blankings for 640x480@15fps. */ 1195 mt9v111->fps = 15; 1196 tpf.numerator = 1; 1197 tpf.denominator = mt9v111->fps; 1198 mt9v111_calc_frame_rate(mt9v111, &tpf); 1199 1200 mt9v111->pwr_count = 0; 1201 mt9v111->addr_space = MT9V111_R01_IFP; 1202 mt9v111->pending = true; 1203 1204 v4l2_i2c_subdev_init(&mt9v111->sd, client, &mt9v111_ops); 1205 1206 #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER) 1207 mt9v111->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE; 1208 mt9v111->sd.entity.ops = &mt9v111_subdev_entity_ops; 1209 mt9v111->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR; 1210 1211 mt9v111->pad.flags = MEDIA_PAD_FL_SOURCE; 1212 ret = media_entity_pads_init(&mt9v111->sd.entity, 1, &mt9v111->pad); 1213 if (ret) 1214 goto error_free_entity; 1215 #endif 1216 1217 ret = mt9v111_chip_probe(mt9v111); 1218 if (ret) 1219 goto error_free_entity; 1220 1221 ret = v4l2_async_register_subdev(&mt9v111->sd); 1222 if (ret) 1223 goto error_free_entity; 1224 1225 return 0; 1226 1227 error_free_entity: 1228 #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER) 1229 media_entity_cleanup(&mt9v111->sd.entity); 1230 #endif 1231 1232 error_free_ctrls: 1233 v4l2_ctrl_handler_free(&mt9v111->ctrls); 1234 1235 mutex_destroy(&mt9v111->pwr_mutex); 1236 mutex_destroy(&mt9v111->stream_mutex); 1237 1238 return ret; 1239 } 1240 1241 static void mt9v111_remove(struct i2c_client *client) 1242 { 1243 struct v4l2_subdev *sd = i2c_get_clientdata(client); 1244 struct mt9v111_dev *mt9v111 = sd_to_mt9v111(sd); 1245 1246 v4l2_async_unregister_subdev(sd); 1247 1248 #if IS_ENABLED(CONFIG_MEDIA_CONTROLLER) 1249 media_entity_cleanup(&sd->entity); 1250 #endif 1251 1252 v4l2_ctrl_handler_free(&mt9v111->ctrls); 1253 1254 mutex_destroy(&mt9v111->pwr_mutex); 1255 mutex_destroy(&mt9v111->stream_mutex); 1256 } 1257 1258 static const struct of_device_id mt9v111_of_match[] = { 1259 { .compatible = "aptina,mt9v111", }, 1260 { /* sentinel */ }, 1261 }; 1262 1263 static struct i2c_driver mt9v111_driver = { 1264 .driver = { 1265 .name = "mt9v111", 1266 .of_match_table = mt9v111_of_match, 1267 }, 1268 .probe = mt9v111_probe, 1269 .remove = mt9v111_remove, 1270 }; 1271 1272 module_i2c_driver(mt9v111_driver); 1273 1274 MODULE_DESCRIPTION("V4L2 sensor driver for Aptina MT9V111"); 1275 MODULE_AUTHOR("Jacopo Mondi <jacopo@jmondi.org>"); 1276 MODULE_LICENSE("GPL v2"); 1277