1284 lines · c
1// SPDX-License-Identifier: GPL-2.02/*3 * V4L2 sensor driver for Aptina MT9V111 image sensor4 * Copyright (C) 2018 Jacopo Mondi <jacopo@jmondi.org>5 *6 * Based on mt9v032 driver7 * Copyright (C) 2010, Laurent Pinchart <laurent.pinchart@ideasonboard.com>8 * Copyright (C) 2008, Guennadi Liakhovetski <kernel@pengutronix.de>9 *10 * Based on mt9v011 driver11 * 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 integrated32 * Image Flow Processing (IFP) engine and a sensor core loosely based on33 * MT9V011.34 *35 * The IFP can produce several output image formats from the sensor core36 * output. This driver currently supports only YUYV format permutations.37 *38 * The driver allows manual frame rate control through set_frame_interval subdev39 * operation or V4L2_CID_V/HBLANK controls, but it is known that the40 * auto-exposure algorithm might modify the programmed frame rate. While the41 * driver initially programs the sensor with auto-exposure and42 * auto-white-balancing enabled, it is possible to disable them and more43 * precisely control the frame rate.44 *45 * While it seems possible to instruct the auto-exposure control algorithm to46 * respect a programmed frame rate when adjusting the pixel integration time,47 * registers controlling this feature are not documented in the public48 * 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 0x8253#define MT9V111_CHIP_ID_LOW 0x3a54 55#define MT9V111_R01_ADDR_SPACE 0x0156#define MT9V111_R01_IFP 0x0157#define MT9V111_R01_CORE 0x0458 59#define MT9V111_IFP_R06_OPMODE_CTRL 0x0660#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 0x0763#define MT9V111_IFP_R07_IFP_RESET_MASK BIT(0)64#define MT9V111_IFP_R08_OUTFMT_CTRL 0x0865#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 0x3a68#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 0xa572#define MT9V111_IFP_RA6_HZOOM 0xa673#define MT9V111_IFP_RA7_HOUT 0xa774#define MT9V111_IFP_RA8_VPAN 0xa875#define MT9V111_IFP_RA9_VZOOM 0xa976#define MT9V111_IFP_RAA_VOUT 0xaa77#define MT9V111_IFP_DECIMATION_MASK GENMASK(9, 0)78#define MT9V111_IFP_DECIMATION_FREEZE BIT(15)79 80#define MT9V111_CORE_R03_WIN_HEIGHT 0x0381#define MT9V111_CORE_R03_WIN_V_OFFS 282#define MT9V111_CORE_R04_WIN_WIDTH 0x0483#define MT9V111_CORE_R04_WIN_H_OFFS 11484#define MT9V111_CORE_R05_HBLANK 0x0585#define MT9V111_CORE_R05_MIN_HBLANK 0x0986#define MT9V111_CORE_R05_MAX_HBLANK GENMASK(9, 0)87#define MT9V111_CORE_R05_DEF_HBLANK 0x2688#define MT9V111_CORE_R06_VBLANK 0x0689#define MT9V111_CORE_R06_MIN_VBLANK 0x0390#define MT9V111_CORE_R06_MAX_VBLANK GENMASK(11, 0)91#define MT9V111_CORE_R06_DEF_VBLANK 0x0492#define MT9V111_CORE_R07_OUT_CTRL 0x0793#define MT9V111_CORE_R07_OUT_CTRL_SAMPLE BIT(4)94#define MT9V111_CORE_R09_PIXEL_INT 0x0995#define MT9V111_CORE_R09_PIXEL_INT_MASK GENMASK(11, 0)96#define MT9V111_CORE_R0D_CORE_RESET 0x0d97#define MT9V111_CORE_R0D_CORE_RESET_MASK BIT(0)98#define MT9V111_CORE_RFF_CHIP_VER 0xff99 100#define MT9V111_PIXEL_ARRAY_WIDTH 640101#define MT9V111_PIXEL_ARRAY_HEIGHT 480102 103#define MT9V111_MAX_CLKIN 27000000104 105/* The default sensor configuration at startup time. */106static 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 117struct 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 configurable162 * in the pixel array sizes range.163 *164 * The desired frame interval, in the supported frame interval range, is165 * obtained by configuring blanking as the sensor does not have a PLL but166 * only a fixed clock divider that generates the output pixel clock.167 */168static 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 185static 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 the191 * full VGA pixel array.192 */193static struct v4l2_rect mt9v111_frame_sizes[] = {194 {195 .width = 640,196 .height = 480,197 },198 {199 .width = 352,200 .height = 288201 },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 218static 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 247static 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 273static 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 300static 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 316static 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 332static 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 359static 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 379static 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 394static 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 408static 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 446static 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 is477 * 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 the485 * above equation.486 *487 * Test all available H/V blank values, until we reach the488 * 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 532static 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 only560 * 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 image592 * 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 AE635 * disabled. If longer than frame time, it affects the output636 * 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 644static 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 we672 * 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 682static 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 are705 * supported and immediately programmed to registers in .s_ctrl706 */707 708 mt9v111->pending = false;709 }710 711 mt9v111->streaming = enable ? true : false;712 mutex_unlock(&mt9v111->stream_mutex);713 714 return 0;715 716error_unlock:717 mutex_unlock(&mt9v111->stream_mutex);718 719 return ret;720}721 722static 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 V4L2735 * 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 else763 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 782static 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 V4L2791 * 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 806static 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 822static 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 834static 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 857static 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 872static 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 890static 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 959done:960 format->format = *__fmt;961 962 mutex_unlock(&mt9v111->stream_mutex);963 964 return 0;965}966 967static 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 975static const struct v4l2_subdev_core_ops mt9v111_core_ops = {976 .s_power = mt9v111_s_power,977};978 979static const struct v4l2_subdev_video_ops mt9v111_video_ops = {980 .s_stream = mt9v111_s_stream,981};982 983static 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 993static const struct v4l2_subdev_ops mt9v111_ops = {994 .core = &mt9v111_core_ops,995 .video = &mt9v111_video_ops,996 .pad = &mt9v111_pad_ops,997};998 999static const struct v4l2_subdev_internal_ops mt9v111_internal_ops = {1000 .init_state = mt9v111_init_state,1001};1002 1003static const struct media_entity_operations mt9v111_subdev_entity_ops = {1004 .link_validate = v4l2_subdev_link_validate,1005};1006 1007/* --- V4L2 ctrl --- */1008static 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 awb1029 * 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 precise1032 * 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 else1042 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 1083static const struct v4l2_ctrl_ops mt9v111_ctrl_ops = {1084 .s_ctrl = mt9v111_s_ctrl,1085};1086 1087static 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 1113power_off:1114 __mt9v111_power_off(&mt9v111->sd);1115 1116 return ret;1117}1118 1119static 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 1237error_free_entity:1238 media_entity_cleanup(&mt9v111->sd.entity);1239 1240error_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 1249static 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 1264static const struct of_device_id mt9v111_of_match[] = {1265 { .compatible = "aptina,mt9v111", },1266 { /* sentinel */ }1267};1268MODULE_DEVICE_TABLE(of, mt9v111_of_match);1269 1270static struct i2c_driver mt9v111_driver = {1271 .driver = {1272 .name = "mt9v111",1273 .of_match_table = mt9v111_of_match,1274 },1275 .probe = mt9v111_probe,1276 .remove = mt9v111_remove,1277};1278 1279module_i2c_driver(mt9v111_driver);1280 1281MODULE_DESCRIPTION("V4L2 sensor driver for Aptina MT9V111");1282MODULE_AUTHOR("Jacopo Mondi <jacopo@jmondi.org>");1283MODULE_LICENSE("GPL v2");1284