drivers/media/platform/starfive/v4l2_driver/imx219_mipi.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * A V4L2 driver for Sony IMX219 cameras.
   4  * Copyright (C) 2019, Raspberry Pi (Trading) Ltd
   5  *
   6  * Based on Sony imx258 camera driver
   7  * Copyright (C) 2018 Intel Corporation
   8  *
   9  * DT / fwnode changes, and regulator / GPIO control taken from imx214 driver
  10  * Copyright 2018 Qtechnology A/S
  11  *
  12  * Flip handling taken from the Sony IMX319 driver.
  13  * Copyright (C) 2018 Intel Corporation
  14  *
  15  * Copyright (C) 2021 StarFive Technology Co., Ltd.
  16  */
  17
  18 #include <linux/clk.h>
  19 #include <linux/delay.h>
  20 #include <linux/gpio/consumer.h>
  21 #include <linux/i2c.h>
  22 #include <linux/module.h>
  23 #include <linux/pm_runtime.h>
  24 #include <linux/regulator/consumer.h>
  25 #include <media/v4l2-ctrls.h>
  26 #include <media/v4l2-device.h>
  27 #include <media/v4l2-event.h>
  28 #include <media/v4l2-fwnode.h>
  29 #include <media/v4l2-mediabus.h>
  30 #include <linux/pinctrl/pinctrl.h>
  31 #include <asm/unaligned.h>
  32
  33 #define IMX219_REG_VALUE_08BIT          1
  34 #define IMX219_REG_VALUE_16BIT          2
  35
  36 #define IMX219_REG_MODE_SELECT          0x0100
  37 #define IMX219_MODE_STANDBY             0x00
  38 #define IMX219_MODE_STREAMING           0x01
  39
  40 /* Chip ID */
  41 #define IMX219_REG_CHIP_ID              0x0000
  42 #define IMX219_CHIP_ID                  0x0219
  43
  44 /* External clock frequency is 24.0M */
  45 #define IMX219_XCLK_FREQ                24000000
  46
  47 /* Pixel rate is fixed at 182.4M for all the modes */
  48 #define IMX219_PIXEL_RATE               182400000
  49
  50 #define IMX219_DEFAULT_LINK_FREQ        456000000
  51
  52 /* V_TIMING internal */
  53 #define IMX219_REG_VTS                  0x0160
  54 #define IMX219_VTS_15FPS                0x0dc6
  55 #define IMX219_VTS_30FPS_1080P          0x06e3
  56 #define IMX219_VTS_30FPS_BINNED         0x06e3
  57 #define IMX219_VTS_30FPS_1280x720       0x06e3
  58 #define IMX219_VTS_30FPS_640x480        0x06e3
  59 #define IMX219_VTS_MAX                  0xffff
  60
  61 #define IMX219_VBLANK_MIN               4
  62
  63 /*Frame Length Line*/
  64 #define IMX219_FLL_MIN                  0x08a6
  65 #define IMX219_FLL_MAX                  0xffff
  66 #define IMX219_FLL_STEP                 1
  67 #define IMX219_FLL_DEFAULT              0x0c98
  68
  69 /* HBLANK control - read only */
  70 #define IMX219_PPL_DEFAULT              3448
  71
  72 /* Exposure control */
  73 #define IMX219_REG_EXPOSURE             0x015a
  74 #define IMX219_EXPOSURE_MIN             4
  75 #define IMX219_EXPOSURE_STEP            1
  76 #define IMX219_EXPOSURE_DEFAULT         0x640
  77 #define IMX219_EXPOSURE_MAX             65535
  78
  79 /* Analog gain control */
  80 #define IMX219_REG_ANALOG_GAIN          0x0157
  81 #define IMX219_ANA_GAIN_MIN             0
  82 #define IMX219_ANA_GAIN_MAX             232
  83 #define IMX219_ANA_GAIN_STEP            1
  84 #define IMX219_ANA_GAIN_DEFAULT         0xd0
  85
  86 /* Digital gain control */
  87 #define IMX219_REG_DIGITAL_GAIN         0x0158
  88 #define IMX219_DGTL_GAIN_MIN            0x0100
  89 #define IMX219_DGTL_GAIN_MAX            0x0fff
  90 #define IMX219_DGTL_GAIN_DEFAULT        0x0100
  91 #define IMX219_DGTL_GAIN_STEP           1
  92
  93 #define IMX219_REG_ORIENTATION          0x0172
  94
  95 /* Test Pattern Control */
  96 #define IMX219_REG_TEST_PATTERN         0x0600
  97 #define IMX219_TEST_PATTERN_DISABLE     0
  98 #define IMX219_TEST_PATTERN_SOLID_COLOR 1
  99 #define IMX219_TEST_PATTERN_COLOR_BARS  2
 100 #define IMX219_TEST_PATTERN_GREY_COLOR  3
 101 #define IMX219_TEST_PATTERN_PN9         4
 102
 103 /* Test pattern colour components */
 104 #define IMX219_REG_TESTP_RED            0x0602
 105 #define IMX219_REG_TESTP_GREENR         0x0604
 106 #define IMX219_REG_TESTP_BLUE           0x0606
 107 #define IMX219_REG_TESTP_GREENB         0x0608
 108 #define IMX219_TESTP_COLOUR_MIN         0
 109 #define IMX219_TESTP_COLOUR_MAX         0x03ff
 110 #define IMX219_TESTP_COLOUR_STEP        1
 111 #define IMX219_TESTP_RED_DEFAULT        IMX219_TESTP_COLOUR_MAX
 112 #define IMX219_TESTP_GREENR_DEFAULT     0
 113 #define IMX219_TESTP_BLUE_DEFAULT       0
 114 #define IMX219_TESTP_GREENB_DEFAULT     0
 115
 116 /* IMX219 native and active pixel array size. */
 117 #define IMX219_NATIVE_WIDTH             3296U
 118 #define IMX219_NATIVE_HEIGHT            2480U
 119 #define IMX219_PIXEL_ARRAY_LEFT         8U
 120 #define IMX219_PIXEL_ARRAY_TOP          8U
 121 #define IMX219_PIXEL_ARRAY_WIDTH        3280U
 122 #define IMX219_PIXEL_ARRAY_HEIGHT       2464U
 123
 124 struct imx219_reg {
 125         u16 address;
 126         u8 val;
 127 };
 128
 129 struct imx219_reg_list {
 130         unsigned int num_of_regs;
 131         const struct imx219_reg *regs;
 132 };
 133
 134 /* Mode : resolution and related config&values */
 135 struct imx219_mode {
 136         /* Frame width */
 137         unsigned int width;
 138         /* Frame height */
 139         unsigned int height;
 140
 141         unsigned int fps;
 142
 143         /* Analog crop rectangle. */
 144         struct v4l2_rect crop;
 145
 146         /* V-timing */
 147         unsigned int vts_def;
 148
 149         /* Default register values */
 150         struct imx219_reg_list reg_list;
 151 };
 152
 153 /*
 154  * Register sets lifted off the i2C interface from the Raspberry Pi firmware
 155  * driver.
 156  * 3280x2464 = mode 2, 1920x1080 = mode 1, 1640x1232 = mode 4, 640x480 = mode 7.
 157  */
 158
 159 static const struct imx219_reg mode_1920_1080_regs[] = {
 160         {0x0100, 0x00},
 161         {0x30eb, 0x05},
 162         {0x30eb, 0x0c},
 163         {0x300a, 0xff},
 164         {0x300b, 0xff},
 165         {0x30eb, 0x05},
 166         {0x30eb, 0x09},
 167         {0x0114, 0x01},
 168         {0x0128, 0x00},
 169         {0x012a, 0x18},
 170         {0x012b, 0x00},
 171         {0x0162, 0x0d},
 172         {0x0163, 0x78},
 173         {0x0164, 0x02},
 174         {0x0165, 0xa8},
 175         {0x0166, 0x0a},
 176         {0x0167, 0x27},
 177         {0x0168, 0x02},
 178         {0x0169, 0xb4},
 179         {0x016a, 0x06},
 180         {0x016b, 0xeb},
 181         {0x016c, 0x07},
 182         {0x016d, 0x80},
 183         {0x016e, 0x04},
 184         {0x016f, 0x38},
 185         {0x0170, 0x01},
 186         {0x0171, 0x01},
 187         {0x0174, 0x00},
 188         {0x0175, 0x00},
 189         {0x0301, 0x05},
 190         {0x0303, 0x01},
 191         {0x0304, 0x03},
 192         {0x0305, 0x03},
 193         {0x0306, 0x00},
 194         {0x0307, 0x39},
 195         {0x030b, 0x01},
 196         {0x030c, 0x00},
 197         {0x030d, 0x72},
 198         {0x0624, 0x07},
 199         {0x0625, 0x80},
 200         {0x0626, 0x04},
 201         {0x0627, 0x38},
 202         {0x455e, 0x00},
 203         {0x471e, 0x4b},
 204         {0x4767, 0x0f},
 205         {0x4750, 0x14},
 206         {0x4540, 0x00},
 207         {0x47b4, 0x14},
 208         {0x4713, 0x30},
 209         {0x478b, 0x10},
 210         {0x478f, 0x10},
 211         {0x4793, 0x10},
 212         {0x4797, 0x0e},
 213         {0x479b, 0x0e},
 214 };
 215
 216 static const struct imx219_reg mode_1280_720_regs[] = {
 217         {0x0100, 0x00},
 218         {0x30eb, 0x05},
 219         {0x30eb, 0x0c},
 220         {0x300a, 0xff},
 221         {0x300b, 0xff},
 222         {0x30eb, 0x05},
 223         {0x30eb, 0x09},
 224         {0x0114, 0x01},
 225         {0x0128, 0x00},
 226         {0x012a, 0x18},
 227         {0x012b, 0x00},
 228         {0x0162, 0x0d},
 229         {0x0163, 0x78},
 230         {0x0164, 0x01},
 231         {0x0165, 0x68},
 232         {0x0166, 0x0b},
 233         {0x0167, 0x67},
 234         {0x0168, 0x02},
 235         {0x0169, 0x00},
 236         {0x016a, 0x07},
 237         {0x016b, 0xf9},
 238         {0x016c, 0x05},
 239         {0x016d, 0x00},
 240         {0x016e, 0x02},
 241         {0x016f, 0xd0},
 242         {0x0170, 0x01},
 243         {0x0171, 0x01},
 244         {0x0174, 0x01},
 245         {0x0175, 0x01},
 246         {0x0301, 0x05},
 247         {0x0303, 0x01},
 248         {0x0304, 0x03},
 249         {0x0305, 0x03},
 250         {0x0306, 0x00},
 251         {0x0307, 0x39},
 252         {0x030b, 0x01},
 253         {0x030c, 0x00},
 254         {0x030d, 0x72},
 255         {0x0624, 0x06},
 256         {0x0625, 0x68},
 257         {0x0626, 0x04},
 258         {0x0627, 0xd0},
 259         {0x455e, 0x00},
 260         {0x471e, 0x4b},
 261         {0x4767, 0x0f},
 262         {0x4750, 0x14},
 263         {0x4540, 0x00},
 264         {0x47b4, 0x14},
 265         {0x4713, 0x30},
 266         {0x478b, 0x10},
 267         {0x478f, 0x10},
 268         {0x4793, 0x10},
 269         {0x4797, 0x0e},
 270         {0x479b, 0x0e},
 271 };
 272
 273 static const struct imx219_reg mode_640_480_regs[] = {
 274         {0x0100, 0x00},
 275         {0x30eb, 0x05},
 276         {0x30eb, 0x0c},
 277         {0x300a, 0xff},
 278         {0x300b, 0xff},
 279         {0x30eb, 0x05},
 280         {0x30eb, 0x09},
 281         {0x0114, 0x01},
 282         {0x0128, 0x00},
 283         {0x012a, 0x18},
 284         {0x012b, 0x00},
 285         {0x0162, 0x0d},
 286         {0x0163, 0x78},
 287         {0x0164, 0x03},
 288         {0x0165, 0xe8},
 289         {0x0166, 0x08},
 290         {0x0167, 0xe7},
 291         {0x0168, 0x02},
 292         {0x0169, 0xf0},
 293         {0x016a, 0x06},
 294         {0x016b, 0xaf},
 295         {0x016c, 0x02},
 296         {0x016d, 0x80},
 297         {0x016e, 0x01},
 298         {0x016f, 0xe0},
 299         {0x0170, 0x01},
 300         {0x0171, 0x01},
 301         {0x0174, 0x03},
 302         {0x0175, 0x03},
 303         {0x0301, 0x05},
 304         {0x0303, 0x01},
 305         {0x0304, 0x03},
 306         {0x0305, 0x03},
 307         {0x0306, 0x00},
 308         {0x0307, 0x39},
 309         {0x030b, 0x01},
 310         {0x030c, 0x00},
 311         {0x030d, 0x72},
 312         {0x0624, 0x06},
 313         {0x0625, 0x68},
 314         {0x0626, 0x04},
 315         {0x0627, 0xd0},
 316         {0x455e, 0x00},
 317         {0x471e, 0x4b},
 318         {0x4767, 0x0f},
 319         {0x4750, 0x14},
 320         {0x4540, 0x00},
 321         {0x47b4, 0x14},
 322         {0x4713, 0x30},
 323         {0x478b, 0x10},
 324         {0x478f, 0x10},
 325         {0x4793, 0x10},
 326         {0x4797, 0x0e},
 327         {0x479b, 0x0e},
 328 };
 329
 330 static const struct imx219_reg raw8_framefmt_regs[] = {
 331         {0x018c, 0x08},
 332         {0x018d, 0x08},
 333         {0x0309, 0x08},
 334 };
 335
 336 static const struct imx219_reg raw10_framefmt_regs[] = {
 337         {0x018c, 0x0a},
 338         {0x018d, 0x0a},
 339         {0x0309, 0x0a},
 340 };
 341
 342 static const s64 imx219_link_freq_menu[] = {
 343         IMX219_DEFAULT_LINK_FREQ,
 344 };
 345
 346 static const char * const imx219_test_pattern_menu[] = {
 347         "Disabled",
 348         "Color Bars",
 349         "Solid Color",
 350         "Grey Color Bars",
 351         "PN9"
 352 };
 353
 354 static const int imx219_test_pattern_val[] = {
 355         IMX219_TEST_PATTERN_DISABLE,
 356         IMX219_TEST_PATTERN_COLOR_BARS,
 357         IMX219_TEST_PATTERN_SOLID_COLOR,
 358         IMX219_TEST_PATTERN_GREY_COLOR,
 359         IMX219_TEST_PATTERN_PN9,
 360 };
 361
 362 /* regulator supplies */
 363 static const char * const imx219_supply_name[] = {
 364         /* Supplies can be enabled in any order */
 365         "VANA",  /* Analog (2.8V) supply */
 366         "VDIG",  /* Digital Core (1.8V) supply */
 367         "VDDL",  /* IF (1.2V) supply */
 368 };
 369
 370 #define IMX219_NUM_SUPPLIES ARRAY_SIZE(imx219_supply_name)
 371
 372 /*
 373  * The supported formats.
 374  * This table MUST contain 4 entries per format, to cover the various flip
 375  * combinations in the order
 376  * - no flip
 377  * - h flip
 378  * - v flip
 379  * - h&v flips
 380  */
 381 static const u32 codes[] = {
 382         MEDIA_BUS_FMT_SRGGB10_1X10,
 383         MEDIA_BUS_FMT_SGRBG10_1X10,
 384         MEDIA_BUS_FMT_SGBRG10_1X10,
 385         MEDIA_BUS_FMT_SBGGR10_1X10,
 386
 387         MEDIA_BUS_FMT_SRGGB8_1X8,
 388         MEDIA_BUS_FMT_SGRBG8_1X8,
 389         MEDIA_BUS_FMT_SGBRG8_1X8,
 390         MEDIA_BUS_FMT_SBGGR8_1X8,
 391 };
 392
 393 /*
 394  * Initialisation delay between XCLR low->high and the moment when the sensor
 395  * can start capture (i.e. can leave software stanby) must be not less than:
 396  *   t4 + max(t5, t6 + <time to initialize the sensor register over I2C>)
 397  * where
 398  *   t4 is fixed, and is max 200uS,
 399  *   t5 is fixed, and is 6000uS,
 400  *   t6 depends on the sensor external clock, and is max 32000 clock periods.
 401  * As per sensor datasheet, the external clock must be from 6MHz to 27MHz.
 402  * So for any acceptable external clock t6 is always within the range of
 403  * 1185 to 5333 uS, and is always less than t5.
 404  * For this reason this is always safe to wait (t4 + t5) = 6200 uS, then
 405  * initialize the sensor over I2C, and then exit the software standby.
 406  *
 407  * This start-up time can be optimized a bit more, if we start the writes
 408  * over I2C after (t4+t6), but before (t4+t5) expires. But then sensor
 409  * initialization over I2C may complete before (t4+t5) expires, and we must
 410  * ensure that capture is not started before (t4+t5).
 411  *
 412  * This delay doesn't account for the power supply startup time. If needed,
 413  * this should be taken care of via the regulator framework. E.g. in the
 414  * case of DT for regulator-fixed one should define the startup-delay-us
 415  * property.
 416  */
 417 #define IMX219_XCLR_MIN_DELAY_US        6200
 418 #define IMX219_XCLR_DELAY_RANGE_US      1000
 419
 420 /* Mode configs */
 421 static const struct imx219_mode supported_modes[] = {
 422         {
 423                 /* 1080P 30fps cropped */
 424                 .width = 1920,
 425                 .height = 1080,
 426                 .fps = 30,
 427                 .crop = {
 428                         .left = 688,
 429                         .top = 700,
 430                         .width = 1920,
 431                         .height = 1080
 432                 },
 433                 .vts_def = IMX219_VTS_30FPS_1080P,
 434                 .reg_list = {
 435                         .num_of_regs = ARRAY_SIZE(mode_1920_1080_regs),
 436                         .regs = mode_1920_1080_regs,
 437                 },
 438         },
 439         {
 440                 /* 1280x720 30fps mode */
 441                 .width = 1280,
 442                 .height = 720,
 443                 .fps = 30,
 444                 .crop = {
 445                         .left = 360,
 446                         .top = 512,
 447                         .width = 2560,
 448                         .height = 1440
 449                 },
 450                 .vts_def = IMX219_VTS_30FPS_1280x720,
 451                 .reg_list = {
 452                         .num_of_regs = ARRAY_SIZE(mode_1280_720_regs),
 453                         .regs = mode_1280_720_regs,
 454                 },
 455         },
 456         {
 457                 /* 640x480 30fps mode */
 458                 .width = 640,
 459                 .height = 480,
 460                 .fps = 30,
 461                 .crop = {
 462                         .left = 1008,
 463                         .top = 760,
 464                         .width = 1280,
 465                         .height = 960
 466                 },
 467                 .vts_def = IMX219_VTS_30FPS_640x480,
 468                 .reg_list = {
 469                         .num_of_regs = ARRAY_SIZE(mode_640_480_regs),
 470                         .regs = mode_640_480_regs,
 471                 },
 472         },
 473 };
 474
 475 struct sensor_pinctrl_info {
 476         struct pinctrl *pinctrl;
 477         struct pinctrl_state *power_up;
 478         struct pinctrl_state *power_down;
 479         bool use_pinctrl;
 480 };
 481
 482 struct imx219 {
 483         struct v4l2_subdev sd;
 484         struct media_pad pad;
 485         //struct v4l2_fwnode_endpoint ep; /* the parsed DT endpoint info */
 486
 487         struct v4l2_mbus_framefmt fmt;
 488
 489         struct clk *xclk; /* system clock to IMX219 */
 490         u32 xclk_freq;
 491
 492         struct gpio_desc *reset_gpio;
 493         struct regulator_bulk_data supplies[IMX219_NUM_SUPPLIES];
 494
 495         struct v4l2_ctrl_handler ctrl_handler;
 496         /* V4L2 Controls */
 497         struct v4l2_ctrl *pixel_rate;
 498         struct v4l2_ctrl *link_freq;
 499         struct v4l2_ctrl *exposure;
 500         struct v4l2_ctrl *vflip;
 501         struct v4l2_ctrl *hflip;
 502         struct v4l2_ctrl *vblank;
 503         struct v4l2_ctrl *hblank;
 504
 505         /* Current mode */
 506         const struct imx219_mode *mode;
 507         struct v4l2_fract frame_interval;
 508
 509         /*
 510          * Mutex for serialized access:
 511          * Protect sensor module set pad format and start/stop streaming safely.
 512          */
 513         struct mutex mutex;
 514
 515         /* Streaming on/off */
 516         int streaming;
 517
 518         struct sensor_pinctrl_info imx219_pctrl;
 519 };
 520
 521 int imx219_sensor_pinctrl_init(
 522         struct sensor_pinctrl_info *sensor_pctrl, struct device *dev)
 523 {
 524         sensor_pctrl->pinctrl = devm_pinctrl_get(dev);
 525         if (IS_ERR_OR_NULL(sensor_pctrl->pinctrl)) {
 526                 pr_err("Getting pinctrl handle failed\n");
 527                 return -EINVAL;
 528         }
 529
 530         sensor_pctrl->power_up
 531                 = pinctrl_lookup_state(sensor_pctrl->pinctrl, "power_up");
 532         if (IS_ERR_OR_NULL(sensor_pctrl->power_up)) {
 533                 pr_err("Failed to get the power_up pinctrl handle\n");
 534                 return -EINVAL;
 535         }
 536
 537         sensor_pctrl->power_down
 538                 = pinctrl_lookup_state(sensor_pctrl->pinctrl, "power_down");
 539         if (IS_ERR_OR_NULL(sensor_pctrl->power_down)) {
 540                 pr_err("Failed to get the power_down pinctrl handle\n");
 541                 return -EINVAL;
 542         }
 543
 544         sensor_pctrl->use_pinctrl = true;
 545
 546         return 0;
 547 }
 548
 549 static inline struct imx219 *to_imx219(struct v4l2_subdev *_sd)
 550 {
 551         return container_of(_sd, struct imx219, sd);
 552 }
 553
 554 /* Read registers up to 2 at a time */
 555 static int imx219_read_reg(struct imx219 *imx219, u16 reg, u32 len, u32 *val)
 556 {
 557         struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 558         struct i2c_msg msgs[2];
 559         u8 addr_buf[2] = { reg >> 8, reg & 0xff };
 560         u8 data_buf[4] = { 0, };
 561         int ret;
 562
 563         if (len > 4)
 564                 return -EINVAL;
 565
 566         /* Write register address */
 567         msgs[0].addr = client->addr;
 568         msgs[0].flags = 0;
 569         msgs[0].len = ARRAY_SIZE(addr_buf);
 570         msgs[0].buf = addr_buf;
 571
 572         /* Read data from register */
 573         msgs[1].addr = client->addr;
 574         msgs[1].flags = I2C_M_RD;
 575         msgs[1].len = len;
 576         msgs[1].buf = &data_buf[4 - len];
 577
 578         ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
 579         if (ret != ARRAY_SIZE(msgs))
 580                 return -EIO;
 581
 582         *val = get_unaligned_be32(data_buf);
 583
 584         return 0;
 585 }
 586
 587 /* Write registers up to 2 at a time */
 588 static int imx219_write_reg(struct imx219 *imx219, u16 reg, u32 len, u32 val)
 589 {
 590         struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 591         u8 buf[6];
 592
 593         if (len > 4)
 594                 return -EINVAL;
 595
 596         put_unaligned_be16(reg, buf);
 597         put_unaligned_be32(val << (8 * (4 - len)), buf + 2);
 598         if (i2c_master_send(client, buf, len + 2) != len + 2)
 599                 return -EIO;
 600
 601         return 0;
 602 }
 603
 604 /* Write a list of registers */
 605 static int imx219_write_regs(struct imx219 *imx219,
 606                              const struct imx219_reg *regs, u32 len)
 607 {
 608         struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 609         unsigned int i;
 610         int ret;
 611
 612         for (i = 0; i < len; i++) {
 613                 ret = imx219_write_reg(imx219, regs[i].address, 1, regs[i].val);
 614                 if (ret) {
 615                         dev_err_ratelimited(&client->dev,
 616                                             "Failed to write reg 0x%4.4x. error = %d\n",
 617                                             regs[i].address, ret);
 618
 619                         return ret;
 620                 }
 621         }
 622
 623         return 0;
 624 }
 625
 626 /* Get bayer order based on flip setting. */
 627 static u32 imx219_get_format_code(struct imx219 *imx219, u32 code)
 628 {
 629         unsigned int i;
 630
 631         lockdep_assert_held(&imx219->mutex);
 632
 633         for (i = 0; i < ARRAY_SIZE(codes); i++)
 634                 if (codes[i] == code)
 635                         break;
 636
 637         if (i >= ARRAY_SIZE(codes))
 638                 i = 0;
 639
 640         i = (i & ~3) | (imx219->vflip->val ? 2 : 0) |
 641             (imx219->hflip->val ? 1 : 0);
 642
 643         return codes[i];
 644 }
 645
 646 static void imx219_set_default_format(struct imx219 *imx219)
 647 {
 648         struct v4l2_mbus_framefmt *fmt;
 649
 650         fmt = &imx219->fmt;
 651         fmt->code = MEDIA_BUS_FMT_SRGGB10_1X10;
 652         fmt->colorspace = V4L2_COLORSPACE_SRGB;
 653         fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
 654         fmt->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
 655                 fmt->colorspace, fmt->ycbcr_enc);
 656         fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
 657         fmt->width = supported_modes[0].width;
 658         fmt->height = supported_modes[0].height;
 659         fmt->field = V4L2_FIELD_NONE;
 660 }
 661
 662 static int imx219_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
 663 {
 664         struct imx219 *imx219 = to_imx219(sd);
 665         struct v4l2_mbus_framefmt *try_fmt =
 666                 v4l2_subdev_get_try_format(sd, fh->state, 0);
 667         struct v4l2_rect *try_crop;
 668
 669         mutex_lock(&imx219->mutex);
 670
 671         /* Initialize try_fmt */
 672         try_fmt->width = supported_modes[0].width;
 673         try_fmt->height = supported_modes[0].height;
 674         try_fmt->code = imx219_get_format_code(imx219, MEDIA_BUS_FMT_SRGGB10_1X10);
 675         try_fmt->field = V4L2_FIELD_NONE;
 676
 677         /* Initialize try_crop rectangle. */
 678         try_crop = v4l2_subdev_get_try_crop(sd, fh->state, 0);
 679         try_crop->top = IMX219_PIXEL_ARRAY_TOP;
 680         try_crop->left = IMX219_PIXEL_ARRAY_LEFT;
 681         try_crop->width = IMX219_PIXEL_ARRAY_WIDTH;
 682         try_crop->height = IMX219_PIXEL_ARRAY_HEIGHT;
 683
 684         mutex_unlock(&imx219->mutex);
 685
 686         return 0;
 687 }
 688
 689 static int imx219_set_ctrl(struct v4l2_ctrl *ctrl)
 690 {
 691         struct imx219 *imx219 =
 692                 container_of(ctrl->handler, struct imx219, ctrl_handler);
 693         struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 694         int ret;
 695
 696         if (ctrl->id == V4L2_CID_VBLANK) {
 697                 int exposure_max, exposure_def;
 698
 699                 /* Update max exposure while meeting expected vblanking */
 700                 exposure_max = imx219->mode->height + ctrl->val - 4;
 701                 exposure_def = (exposure_max < IMX219_EXPOSURE_DEFAULT) ?
 702                         exposure_max : IMX219_EXPOSURE_DEFAULT;
 703                 __v4l2_ctrl_modify_range(imx219->exposure, imx219->exposure->minimum,
 704                                 exposure_max, imx219->exposure->step, exposure_def);
 705         }
 706
 707         /*
 708          * Applying V4L2 control value only happens
 709          * when power is up for streaming
 710          */
 711         if (pm_runtime_get_if_in_use(&client->dev) == 0)
 712                 return 0;
 713
 714         switch (ctrl->id) {
 715         case V4L2_CID_ANALOGUE_GAIN:
 716                 ret = imx219_write_reg(imx219, IMX219_REG_ANALOG_GAIN,
 717                                 IMX219_REG_VALUE_08BIT, ctrl->val);
 718                 break;
 719         case V4L2_CID_EXPOSURE:
 720                 ret = imx219_write_reg(imx219, IMX219_REG_EXPOSURE,
 721                                 IMX219_REG_VALUE_16BIT, ctrl->val);
 722                 break;
 723         case V4L2_CID_DIGITAL_GAIN:
 724                 ret = imx219_write_reg(imx219, IMX219_REG_DIGITAL_GAIN,
 725                                 IMX219_REG_VALUE_16BIT, ctrl->val);
 726                 break;
 727         case V4L2_CID_TEST_PATTERN:
 728                 ret = imx219_write_reg(imx219, IMX219_REG_TEST_PATTERN,
 729                                 IMX219_REG_VALUE_16BIT, imx219_test_pattern_val[ctrl->val]);
 730                 break;
 731         case V4L2_CID_HFLIP:
 732         case V4L2_CID_VFLIP:
 733                 ret = imx219_write_reg(imx219, IMX219_REG_ORIENTATION, 1,
 734                         imx219->hflip->val | imx219->vflip->val << 1);
 735                 break;
 736         case V4L2_CID_VBLANK:
 737                 ret = imx219_write_reg(imx219, IMX219_REG_VTS, IMX219_REG_VALUE_16BIT,
 738                                 imx219->mode->height + ctrl->val);
 739                 break;
 740         case V4L2_CID_TEST_PATTERN_RED:
 741                 ret = imx219_write_reg(imx219, IMX219_REG_TESTP_RED,
 742                                 IMX219_REG_VALUE_16BIT, ctrl->val);
 743                 break;
 744         case V4L2_CID_TEST_PATTERN_GREENR:
 745                 ret = imx219_write_reg(imx219, IMX219_REG_TESTP_GREENR,
 746                                 IMX219_REG_VALUE_16BIT, ctrl->val);
 747                 break;
 748         case V4L2_CID_TEST_PATTERN_BLUE:
 749                 ret = imx219_write_reg(imx219, IMX219_REG_TESTP_BLUE,
 750                                 IMX219_REG_VALUE_16BIT, ctrl->val);
 751                 break;
 752         case V4L2_CID_TEST_PATTERN_GREENB:
 753                 ret = imx219_write_reg(imx219, IMX219_REG_TESTP_GREENB,
 754                                 IMX219_REG_VALUE_16BIT, ctrl->val);
 755                 break;
 756         default:
 757                 dev_info(&client->dev,
 758                          "ctrl(id:0x%x,val:0x%x) is not handled\n", ctrl->id, ctrl->val);
 759                 ret = -EINVAL;
 760                 break;
 761         }
 762
 763         pm_runtime_put(&client->dev);
 764
 765         return ret;
 766 }
 767
 768 static const struct v4l2_ctrl_ops imx219_ctrl_ops = {
 769         .s_ctrl = imx219_set_ctrl,
 770 };
 771
 772 static int imx219_enum_mbus_code(struct v4l2_subdev *sd,
 773                                  struct v4l2_subdev_state *state,
 774                                  struct v4l2_subdev_mbus_code_enum *code)
 775 {
 776         struct imx219 *imx219 = to_imx219(sd);
 777
 778         if (code->index >= (ARRAY_SIZE(codes) / 4))
 779                 return -EINVAL;
 780
 781         mutex_lock(&imx219->mutex);
 782         code->code = imx219_get_format_code(imx219, codes[code->index * 4]);
 783         mutex_unlock(&imx219->mutex);
 784
 785         return 0;
 786 }
 787
 788 static int imx219_enum_frame_size(struct v4l2_subdev *sd,
 789                                   struct v4l2_subdev_state *state,
 790                                   struct v4l2_subdev_frame_size_enum *fse)
 791 {
 792         struct imx219 *imx219 = to_imx219(sd);
 793         u32 code;
 794
 795         if (fse->index >= ARRAY_SIZE(supported_modes))
 796                 return -EINVAL;
 797
 798         mutex_lock(&imx219->mutex);
 799         code = imx219_get_format_code(imx219, fse->code);
 800         mutex_unlock(&imx219->mutex);
 801         if (fse->code != code)
 802                 return -EINVAL;
 803
 804         fse->min_width = supported_modes[fse->index].width;
 805         fse->max_width = fse->min_width;
 806         fse->min_height = supported_modes[fse->index].height;
 807         fse->max_height = fse->min_height;
 808
 809         return 0;
 810 }
 811
 812 static int imx219_try_frame_interval(struct imx219 *imx219,
 813                                      struct v4l2_fract *fi,
 814                                      u32 w, u32 h)
 815 {
 816         const struct imx219_mode *mode;
 817
 818         mode = v4l2_find_nearest_size(supported_modes, ARRAY_SIZE(supported_modes),
 819                         width, height, w, h);
 820         if (!mode || (mode->width != w || mode->height != h))
 821                 return -EINVAL;
 822
 823         fi->numerator = 1;
 824         fi->denominator = mode->fps;
 825
 826         return mode->fps;
 827 }
 828
 829 static int imx219_enum_frame_interval(struct v4l2_subdev *sd,
 830                         struct v4l2_subdev_state *state,
 831                         struct v4l2_subdev_frame_interval_enum *fie)
 832 {
 833         struct imx219 *imx219 = to_imx219(sd);
 834         struct v4l2_fract tpf;
 835         u32 code;
 836         int ret;
 837
 838         if (fie->index >= ARRAY_SIZE(supported_modes))
 839                 return -EINVAL;
 840
 841         mutex_lock(&imx219->mutex);
 842         code = imx219_get_format_code(imx219, fie->code);
 843         if (fie->code != code) {
 844                 ret = -EINVAL;
 845                 goto out;
 846         }
 847
 848         ret = imx219_try_frame_interval(imx219, &tpf,
 849                                 fie->width, fie->height);
 850         if (ret < 0) {
 851                 ret = -EINVAL;
 852                 goto out;
 853         }
 854
 855         mutex_unlock(&imx219->mutex);
 856         fie->interval = tpf;
 857
 858         return 0;
 859
 860 out:
 861         mutex_unlock(&imx219->mutex);
 862         return ret;
 863 }
 864
 865 static int imx219_g_frame_interval(struct v4l2_subdev *sd,
 866                                    struct v4l2_subdev_frame_interval *fi)
 867 {
 868         struct imx219 *imx219 = to_imx219(sd);
 869
 870         mutex_lock(&imx219->mutex);
 871         fi->interval = imx219->frame_interval;
 872         mutex_unlock(&imx219->mutex);
 873
 874         return 0;
 875 }
 876
 877 static int imx219_s_frame_interval(struct v4l2_subdev *sd,
 878                                    struct v4l2_subdev_frame_interval *fi)
 879 {
 880         struct imx219 *imx219 = to_imx219(sd);
 881         const struct imx219_mode *mode = imx219->mode;
 882         int frame_rate, ret = 0;
 883
 884         if (fi->pad != 0)
 885                 return -EINVAL;
 886
 887         mutex_lock(&imx219->mutex);
 888
 889         if (imx219->streaming) {
 890                 ret = -EBUSY;
 891                 goto out;
 892         }
 893
 894         frame_rate = imx219_try_frame_interval(imx219, &fi->interval,
 895                                                mode->width, mode->height);
 896         if (frame_rate < 0) {
 897                 ret = -EINVAL;
 898                 goto out;
 899         }
 900
 901         imx219->frame_interval = fi->interval;
 902
 903 out:
 904         mutex_unlock(&imx219->mutex);
 905         return ret;
 906 }
 907
 908 static void imx219_reset_colorspace(struct v4l2_mbus_framefmt *fmt)
 909 {
 910         fmt->colorspace = V4L2_COLORSPACE_SRGB;
 911         fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
 912         fmt->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
 913                         fmt->colorspace, fmt->ycbcr_enc);
 914         fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
 915 }
 916
 917 static void imx219_update_pad_format(struct imx219 *imx219,
 918                                      const struct imx219_mode *mode,
 919                                      struct v4l2_subdev_format *fmt)
 920 {
 921         fmt->format.width = mode->width;
 922         fmt->format.height = mode->height;
 923         fmt->format.field = V4L2_FIELD_NONE;
 924         imx219_reset_colorspace(&fmt->format);
 925 }
 926
 927 static int __imx219_get_pad_format(struct imx219 *imx219,
 928                                    struct v4l2_subdev_state *state,
 929                                    struct v4l2_subdev_format *fmt)
 930 {
 931         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
 932                 struct v4l2_mbus_framefmt *try_fmt =
 933                         v4l2_subdev_get_try_format(&imx219->sd, state, fmt->pad);
 934                 /* update the code which could change due to vflip or hflip: */
 935                 try_fmt->code = imx219_get_format_code(imx219, try_fmt->code);
 936                 fmt->format = *try_fmt;
 937         } else {
 938                 imx219_update_pad_format(imx219, imx219->mode, fmt);
 939                 fmt->format.code = imx219_get_format_code(imx219, imx219->fmt.code);
 940         }
 941
 942         return 0;
 943 }
 944
 945 static int imx219_get_pad_format(struct v4l2_subdev *sd,
 946                                  struct v4l2_subdev_state *state,
 947                                  struct v4l2_subdev_format *fmt)
 948 {
 949         struct imx219 *imx219 = to_imx219(sd);
 950         int ret;
 951
 952         mutex_lock(&imx219->mutex);
 953         ret = __imx219_get_pad_format(imx219, state, fmt);
 954         mutex_unlock(&imx219->mutex);
 955
 956         return ret;
 957 }
 958
 959 static int imx219_set_pad_format(struct v4l2_subdev *sd,
 960                                 struct v4l2_subdev_state *state,
 961                                 struct v4l2_subdev_format *fmt)
 962 {
 963         struct imx219 *imx219 = to_imx219(sd);
 964         const struct imx219_mode *mode;
 965         struct v4l2_mbus_framefmt *framefmt;
 966         int exposure_max, exposure_def, hblank;
 967         unsigned int i;
 968
 969         mutex_lock(&imx219->mutex);
 970
 971         for (i = 0; i < ARRAY_SIZE(codes); i++)
 972                 if (codes[i] == fmt->format.code)
 973                         break;
 974         if (i >= ARRAY_SIZE(codes))
 975                 i = 0;
 976
 977         /* Bayer order varies with flips */
 978         fmt->format.code = imx219_get_format_code(imx219, codes[i]);
 979
 980         mode = v4l2_find_nearest_size(supported_modes, ARRAY_SIZE(supported_modes),
 981                         width, height, fmt->format.width, fmt->format.height);
 982         imx219_update_pad_format(imx219, mode, fmt);
 983         if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
 984                 framefmt = v4l2_subdev_get_try_format(sd, state, fmt->pad);
 985                 *framefmt = fmt->format;
 986         } else if (imx219->mode != mode ||
 987                    imx219->fmt.code != fmt->format.code) {
 988                 imx219->fmt = fmt->format;
 989                 imx219->mode = mode;
 990                 /* Update limits and set FPS to default */
 991                 __v4l2_ctrl_modify_range(imx219->vblank, IMX219_VBLANK_MIN,
 992                                 IMX219_VTS_MAX - mode->height, 1,
 993                                 mode->vts_def - mode->height);
 994                 __v4l2_ctrl_s_ctrl(imx219->vblank, mode->vts_def - mode->height);
 995                 /* Update max exposure while meeting expected vblanking */
 996                 exposure_max = mode->vts_def - 4;
 997                 exposure_def = (exposure_max < IMX219_EXPOSURE_DEFAULT) ?
 998                         exposure_max : IMX219_EXPOSURE_DEFAULT;
 999                 __v4l2_ctrl_modify_range(imx219->exposure, imx219->exposure->minimum,
1000                                 exposure_max, imx219->exposure->step, exposure_def);
1001                 /*
1002                  * Currently PPL is fixed to IMX219_PPL_DEFAULT, so hblank
1003                  * depends on mode->width only, and is not changeble in any
1004                  * way other than changing the mode.
1005                  */
1006                 hblank = IMX219_PPL_DEFAULT - mode->width;
1007                 __v4l2_ctrl_modify_range(imx219->hblank, hblank, hblank, 1, hblank);
1008         }
1009
1010         mutex_unlock(&imx219->mutex);
1011
1012         return 0;
1013 }
1014
1015 static int imx219_set_framefmt(struct imx219 *imx219)
1016 {
1017         switch (imx219->fmt.code) {
1018         case MEDIA_BUS_FMT_SRGGB8_1X8:
1019         case MEDIA_BUS_FMT_SGRBG8_1X8:
1020         case MEDIA_BUS_FMT_SGBRG8_1X8:
1021         case MEDIA_BUS_FMT_SBGGR8_1X8:
1022                 return imx219_write_regs(imx219, raw8_framefmt_regs,
1023                                 ARRAY_SIZE(raw8_framefmt_regs));
1024
1025         case MEDIA_BUS_FMT_SRGGB10_1X10:
1026         case MEDIA_BUS_FMT_SGRBG10_1X10:
1027         case MEDIA_BUS_FMT_SGBRG10_1X10:
1028         case MEDIA_BUS_FMT_SBGGR10_1X10:
1029                 return imx219_write_regs(imx219, raw10_framefmt_regs,
1030                                 ARRAY_SIZE(raw10_framefmt_regs));
1031         }
1032
1033         return -EINVAL;
1034 }
1035
1036 static const struct v4l2_rect *
1037 __imx219_get_pad_crop(struct imx219 *imx219, struct v4l2_subdev_state *state,
1038                                 unsigned int pad, enum v4l2_subdev_format_whence which)
1039 {
1040         switch (which) {
1041         case V4L2_SUBDEV_FORMAT_TRY:
1042                 return v4l2_subdev_get_try_crop(&imx219->sd, state, pad);
1043         case V4L2_SUBDEV_FORMAT_ACTIVE:
1044                 return &imx219->mode->crop;
1045         }
1046
1047         return NULL;
1048 }
1049
1050 static int imx219_get_selection(struct v4l2_subdev *sd,
1051                                         struct v4l2_subdev_state *state,
1052                                         struct v4l2_subdev_selection *sel)
1053 {
1054         switch (sel->target) {
1055         case V4L2_SEL_TGT_CROP: {
1056                 struct imx219 *imx219 = to_imx219(sd);
1057
1058                 mutex_lock(&imx219->mutex);
1059                 sel->r = *__imx219_get_pad_crop(imx219, state, sel->pad, sel->which);
1060                 mutex_unlock(&imx219->mutex);
1061                 return 0;
1062         }
1063
1064         case V4L2_SEL_TGT_NATIVE_SIZE:
1065                 sel->r.top = 0;
1066                 sel->r.left = 0;
1067                 sel->r.width = IMX219_NATIVE_WIDTH;
1068                 sel->r.height = IMX219_NATIVE_HEIGHT;
1069                 return 0;
1070
1071         case V4L2_SEL_TGT_CROP_DEFAULT:
1072         case V4L2_SEL_TGT_CROP_BOUNDS:
1073                 sel->r.top = IMX219_PIXEL_ARRAY_TOP;
1074                 sel->r.left = IMX219_PIXEL_ARRAY_LEFT;
1075                 sel->r.width = IMX219_PIXEL_ARRAY_WIDTH;
1076                 sel->r.height = IMX219_PIXEL_ARRAY_HEIGHT;
1077                 return 0;
1078         }
1079
1080         return -EINVAL;
1081 }
1082
1083 static int imx219_start_streaming(struct imx219 *imx219)
1084 {
1085         struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
1086         const struct imx219_reg_list *reg_list;
1087         int ret;
1088
1089         ret = pm_runtime_get_sync(&client->dev);
1090         if (ret < 0) {
1091                 pm_runtime_put_noidle(&client->dev);
1092                 return ret;
1093         }
1094
1095         /* Apply default values of current mode */
1096         reg_list = &imx219->mode->reg_list;
1097         ret = imx219_write_regs(imx219, reg_list->regs, reg_list->num_of_regs);
1098         if (ret) {
1099                 dev_err(&client->dev, "%s failed to set mode\n", __func__);
1100                 goto err_rpm_put;
1101         }
1102
1103         ret = imx219_set_framefmt(imx219);
1104         if (ret) {
1105                 dev_err(&client->dev, "%s failed to set frame format: %d\n",
1106                         __func__, ret);
1107                 goto err_rpm_put;
1108         }
1109
1110         /* Apply customized values from user */
1111         ret =  __v4l2_ctrl_handler_setup(imx219->sd.ctrl_handler);
1112         if (ret)
1113                 goto err_rpm_put;
1114
1115         /* set stream on register */
1116         ret = imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
1117                                IMX219_REG_VALUE_08BIT, IMX219_MODE_STREAMING);
1118         if (ret)
1119                 goto err_rpm_put;
1120
1121         /* vflip and hflip cannot change during streaming */
1122         __v4l2_ctrl_grab(imx219->vflip, true);
1123         __v4l2_ctrl_grab(imx219->hflip, true);
1124
1125         return 0;
1126
1127 err_rpm_put:
1128         pm_runtime_put(&client->dev);
1129         return ret;
1130 }
1131
1132 static void imx219_stop_streaming(struct imx219 *imx219)
1133 {
1134         struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
1135         int ret;
1136
1137         /* set stream off register */
1138         ret = imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
1139                         IMX219_REG_VALUE_08BIT, IMX219_MODE_STANDBY);
1140         if (ret)
1141                 dev_err(&client->dev, "%s failed to set stream\n", __func__);
1142
1143         __v4l2_ctrl_grab(imx219->vflip, false);
1144         __v4l2_ctrl_grab(imx219->hflip, false);
1145
1146         pm_runtime_put(&client->dev);
1147 }
1148
1149 static int imx219_set_stream(struct v4l2_subdev *sd, int enable)
1150 {
1151         struct imx219 *imx219 = to_imx219(sd);
1152         int ret = 0;
1153
1154         mutex_lock(&imx219->mutex);
1155         if (imx219->streaming && enable)
1156                 goto unlock;
1157
1158         if (enable) {
1159                 /*
1160                  * Apply default & customized values
1161                  * and then start streaming.
1162                  */
1163                 ret = imx219_start_streaming(imx219);
1164                 if (ret)
1165                         goto err_unlock;
1166         } else {
1167                 imx219_stop_streaming(imx219);
1168         }
1169
1170 unlock:
1171         imx219->streaming += enable ? 1 : -1;
1172         WARN_ON(imx219->streaming < 0);
1173
1174         mutex_unlock(&imx219->mutex);
1175
1176         return ret;
1177
1178 err_unlock:
1179         mutex_unlock(&imx219->mutex);
1180
1181         return ret;
1182 }
1183
1184 /* Power/clock management functions */
1185 static int imx219_power_on(struct device *dev)
1186 {
1187         struct v4l2_subdev *sd = dev_get_drvdata(dev);
1188         struct imx219 *imx219 = to_imx219(sd);
1189         struct sensor_pinctrl_info *sensor_pctrl = &imx219->imx219_pctrl;
1190         int ret;
1191
1192         ret = regulator_bulk_enable(IMX219_NUM_SUPPLIES, imx219->supplies);
1193         if (ret) {
1194                 dev_err(dev, "%s: failed to enable regulators\n",
1195                         __func__);
1196                 return ret;
1197         }
1198
1199         ret = clk_prepare_enable(imx219->xclk);
1200         if (ret) {
1201                 dev_err(dev, "%s: failed to enable clock\n", __func__);
1202                 goto reg_off;
1203         }
1204
1205         if (sensor_pctrl->use_pinctrl) {
1206                 ret = pinctrl_select_state(
1207                         sensor_pctrl->pinctrl,
1208                         sensor_pctrl->power_up);
1209                 if (ret)
1210                         pr_err("cannot set pin to power up\n");
1211         } else
1212                 gpiod_set_value_cansleep(imx219->reset_gpio, 1);
1213
1214         usleep_range(IMX219_XCLR_MIN_DELAY_US,
1215                         IMX219_XCLR_MIN_DELAY_US + IMX219_XCLR_DELAY_RANGE_US);
1216
1217         return 0;
1218
1219 reg_off:
1220         regulator_bulk_disable(IMX219_NUM_SUPPLIES, imx219->supplies);
1221
1222         return ret;
1223 }
1224
1225 static int imx219_power_off(struct device *dev)
1226 {
1227         struct v4l2_subdev *sd = dev_get_drvdata(dev);
1228         struct imx219 *imx219 = to_imx219(sd);
1229         struct sensor_pinctrl_info *sensor_pctrl = &imx219->imx219_pctrl;
1230         int ret;
1231
1232         if (sensor_pctrl->use_pinctrl) {
1233                 ret = pinctrl_select_state(
1234                         sensor_pctrl->pinctrl,
1235                         sensor_pctrl->power_down);
1236                 if (ret)
1237                         pr_err("cannot set pin to power_down\n");
1238         } else
1239                 gpiod_set_value_cansleep(imx219->reset_gpio, 0);
1240
1241         regulator_bulk_disable(IMX219_NUM_SUPPLIES, imx219->supplies);
1242         clk_disable_unprepare(imx219->xclk);
1243
1244         return 0;
1245 }
1246
1247 static int __maybe_unused imx219_suspend(struct device *dev)
1248 {
1249         struct v4l2_subdev *sd = dev_get_drvdata(dev);
1250         struct imx219 *imx219 = to_imx219(sd);
1251
1252         if (imx219->streaming)
1253                 imx219_stop_streaming(imx219);
1254
1255         return 0;
1256 }
1257
1258 static int __maybe_unused imx219_resume(struct device *dev)
1259 {
1260         struct v4l2_subdev *sd = dev_get_drvdata(dev);
1261         struct imx219 *imx219 = to_imx219(sd);
1262         int ret;
1263
1264         if (imx219->streaming) {
1265                 ret = imx219_start_streaming(imx219);
1266                 if (ret)
1267                         goto error;
1268         }
1269
1270         return 0;
1271
1272 error:
1273         imx219_stop_streaming(imx219);
1274         imx219->streaming = false;
1275
1276         return ret;
1277 }
1278
1279 static int imx219_get_regulators(struct imx219 *imx219)
1280 {
1281         struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
1282         unsigned int i;
1283
1284         for (i = 0; i < IMX219_NUM_SUPPLIES; i++)
1285                 imx219->supplies[i].supply = imx219_supply_name[i];
1286
1287         return devm_regulator_bulk_get(&client->dev,
1288                         IMX219_NUM_SUPPLIES, imx219->supplies);
1289 }
1290
1291 /* Verify chip ID */
1292 static int imx219_identify_module(struct imx219 *imx219)
1293 {
1294         struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
1295         int ret;
1296         u32 val;
1297
1298         ret = imx219_read_reg(imx219, IMX219_REG_CHIP_ID,
1299                               IMX219_REG_VALUE_16BIT, &val);
1300         if (ret) {
1301                 dev_err(&client->dev, "failed to read chip id %x\n",
1302                         IMX219_CHIP_ID);
1303                 return ret;
1304         }
1305
1306         if (val != IMX219_CHIP_ID) {
1307                 dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
1308                         IMX219_CHIP_ID, val);
1309                 return -EIO;
1310         }
1311
1312         dev_err(&client->dev, "%s: chip identifier, got 0x%x\n",
1313                 __func__, IMX219_CHIP_ID);
1314
1315         return 0;
1316 }
1317
1318 static const struct v4l2_subdev_core_ops imx219_core_ops = {
1319         .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
1320         .unsubscribe_event = v4l2_event_subdev_unsubscribe,
1321 };
1322
1323 static const struct v4l2_subdev_video_ops imx219_video_ops = {
1324         .g_frame_interval = imx219_g_frame_interval,
1325         .s_frame_interval = imx219_s_frame_interval,
1326         .s_stream = imx219_set_stream,
1327 };
1328
1329 static const struct v4l2_subdev_pad_ops imx219_pad_ops = {
1330         .enum_mbus_code = imx219_enum_mbus_code,
1331         .get_fmt = imx219_get_pad_format,
1332         .set_fmt = imx219_set_pad_format,
1333         .get_selection = imx219_get_selection,
1334         .enum_frame_size = imx219_enum_frame_size,
1335         .enum_frame_interval = imx219_enum_frame_interval,
1336 };
1337
1338 static const struct v4l2_subdev_ops imx219_subdev_ops = {
1339         .core = &imx219_core_ops,
1340         .video = &imx219_video_ops,
1341         .pad = &imx219_pad_ops,
1342 };
1343
1344 static const struct v4l2_subdev_internal_ops imx219_internal_ops = {
1345         .open = imx219_open,
1346 };
1347
1348 /* Initialize control handlers */
1349 static int imx219_init_controls(struct imx219 *imx219)
1350 {
1351         struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
1352         struct v4l2_ctrl_handler *ctrl_hdlr;
1353         unsigned int height = imx219->mode->height;
1354         struct v4l2_fwnode_device_properties props;
1355         int exposure_max, exposure_def, hblank;
1356         int i, ret;
1357
1358         ctrl_hdlr = &imx219->ctrl_handler;
1359         ret = v4l2_ctrl_handler_init(ctrl_hdlr, 12);
1360         if (ret)
1361                 return ret;
1362
1363         mutex_init(&imx219->mutex);
1364         ctrl_hdlr->lock = &imx219->mutex;
1365
1366         /* By default, PIXEL_RATE is read only */
1367         imx219->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
1368                         V4L2_CID_PIXEL_RATE, IMX219_PIXEL_RATE,
1369                         IMX219_PIXEL_RATE, 1, IMX219_PIXEL_RATE);
1370
1371         imx219->link_freq =
1372                 v4l2_ctrl_new_int_menu(ctrl_hdlr, &imx219_ctrl_ops, V4L2_CID_LINK_FREQ,
1373                         ARRAY_SIZE(imx219_link_freq_menu) - 1, 0, imx219_link_freq_menu);
1374         if (imx219->link_freq)
1375                 imx219->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1376
1377         /* Initial vblank/hblank/exposure parameters based on current mode */
1378         imx219->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
1379                                         V4L2_CID_VBLANK, IMX219_VBLANK_MIN,
1380                                         IMX219_VTS_MAX - height, 1,
1381                                         imx219->mode->vts_def - height);
1382         hblank = IMX219_PPL_DEFAULT - imx219->mode->width;
1383         imx219->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
1384                                         V4L2_CID_HBLANK, hblank, hblank, 1, hblank);
1385         if (imx219->hblank)
1386                 imx219->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1387         exposure_max = imx219->mode->vts_def - 4;
1388         exposure_def = (exposure_max < IMX219_EXPOSURE_DEFAULT) ?
1389                 exposure_max : IMX219_EXPOSURE_DEFAULT;
1390         imx219->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
1391                                 V4L2_CID_EXPOSURE, IMX219_EXPOSURE_MIN, exposure_max,
1392                                 IMX219_EXPOSURE_STEP, exposure_def);
1393
1394         v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
1395                         IMX219_ANA_GAIN_MIN, IMX219_ANA_GAIN_MAX,
1396                         IMX219_ANA_GAIN_STEP, IMX219_ANA_GAIN_DEFAULT);
1397
1398         v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
1399                         IMX219_DGTL_GAIN_MIN, IMX219_DGTL_GAIN_MAX,
1400                         IMX219_DGTL_GAIN_STEP, IMX219_DGTL_GAIN_DEFAULT);
1401
1402         imx219->hflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
1403                                           V4L2_CID_HFLIP, 0, 1, 1, 0);
1404         if (imx219->hflip)
1405                 imx219->hflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
1406
1407         imx219->vflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
1408                                           V4L2_CID_VFLIP, 0, 1, 1, 0);
1409         if (imx219->vflip)
1410                 imx219->vflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
1411
1412         v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &imx219_ctrl_ops, V4L2_CID_TEST_PATTERN,
1413                         ARRAY_SIZE(imx219_test_pattern_menu) - 1,
1414                         0, 0, imx219_test_pattern_menu);
1415         for (i = 0; i < 4; i++) {
1416                 /*
1417                  * The assumption is that
1418                  * V4L2_CID_TEST_PATTERN_GREENR == V4L2_CID_TEST_PATTERN_RED + 1
1419                  * V4L2_CID_TEST_PATTERN_BLUE   == V4L2_CID_TEST_PATTERN_RED + 2
1420                  * V4L2_CID_TEST_PATTERN_GREENB == V4L2_CID_TEST_PATTERN_RED + 3
1421                  */
1422                 v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
1423                                 V4L2_CID_TEST_PATTERN_RED + i,
1424                                 IMX219_TESTP_COLOUR_MIN,
1425                                 IMX219_TESTP_COLOUR_MAX,
1426                                 IMX219_TESTP_COLOUR_STEP,
1427                                 IMX219_TESTP_COLOUR_MAX);
1428                 /* The "Solid color" pattern is white by default */
1429         }
1430
1431         if (ctrl_hdlr->error) {
1432                 ret = ctrl_hdlr->error;
1433                 dev_err(&client->dev, "%s control init failed (%d)\n",
1434                         __func__, ret);
1435                 goto error;
1436         }
1437
1438         ret = v4l2_fwnode_device_parse(&client->dev, &props);
1439         if (ret)
1440                 goto error;
1441
1442         ret = v4l2_ctrl_new_fwnode_properties(ctrl_hdlr, &imx219_ctrl_ops, &props);
1443         if (ret)
1444                 goto error;
1445
1446         imx219->sd.ctrl_handler = ctrl_hdlr;
1447
1448         return 0;
1449
1450 error:
1451         v4l2_ctrl_handler_free(ctrl_hdlr);
1452         mutex_destroy(&imx219->mutex);
1453
1454         return ret;
1455 }
1456
1457 static void imx219_free_controls(struct imx219 *imx219)
1458 {
1459         v4l2_ctrl_handler_free(imx219->sd.ctrl_handler);
1460         mutex_destroy(&imx219->mutex);
1461 }
1462
1463 static int imx219_check_hwcfg(struct device *dev)
1464 {
1465         struct fwnode_handle *endpoint;
1466         struct v4l2_fwnode_endpoint ep_cfg = {
1467                 .bus_type = V4L2_MBUS_CSI2_DPHY
1468         };
1469         int ret = -EINVAL;
1470
1471         endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(dev), NULL);
1472         if (!endpoint) {
1473                 dev_err(dev, "endpoint node not found\n");
1474                 return -EINVAL;
1475         }
1476
1477         if (v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep_cfg)) {
1478                 dev_err(dev, "could not parse endpoint\n");
1479                 goto error_out;
1480         }
1481
1482         /* Check the number of MIPI CSI2 data lanes */
1483         if (ep_cfg.bus.mipi_csi2.num_data_lanes != 2) {
1484                 dev_err(dev, "only 2 data lanes are currently supported\n");
1485                 goto error_out;
1486         }
1487
1488         /* Check the link frequency set in device tree */
1489         if (!ep_cfg.nr_of_link_frequencies) {
1490                 dev_err(dev, "link-frequency property not found in DT\n");
1491                 goto error_out;
1492         }
1493
1494         if (ep_cfg.nr_of_link_frequencies != 1 ||
1495             ep_cfg.link_frequencies[0] != IMX219_DEFAULT_LINK_FREQ) {
1496                 dev_err(dev, "Link frequency not supported: %lld\n",
1497                         ep_cfg.link_frequencies[0]);
1498                 goto error_out;
1499         }
1500
1501         ret = 0;
1502
1503 error_out:
1504         v4l2_fwnode_endpoint_free(&ep_cfg);
1505         fwnode_handle_put(endpoint);
1506
1507         return ret;
1508 }
1509
1510 static int imx219_probe(struct i2c_client *client)
1511 {
1512         struct device *dev = &client->dev;
1513         struct imx219 *imx219;
1514         int ret;
1515
1516         imx219 = devm_kzalloc(&client->dev, sizeof(*imx219), GFP_KERNEL);
1517         if (!imx219)
1518                 return -ENOMEM;
1519
1520         v4l2_i2c_subdev_init(&imx219->sd, client, &imx219_subdev_ops);
1521
1522         /* Check the hardware configuration in device tree */
1523         if (imx219_check_hwcfg(dev))
1524                 return -EINVAL;
1525
1526         /* Get system clock (xclk) */
1527         imx219->xclk = devm_clk_get(dev, NULL);
1528         if (IS_ERR(imx219->xclk)) {
1529                 dev_err(dev, "failed to get xclk\n");
1530                 return PTR_ERR(imx219->xclk);
1531         }
1532
1533         imx219->xclk_freq = clk_get_rate(imx219->xclk);
1534         if (imx219->xclk_freq != IMX219_XCLK_FREQ) {
1535                 dev_err(dev, "xclk frequency not supported: %d Hz\n",
1536                         imx219->xclk_freq);
1537                 return -EINVAL;
1538         }
1539
1540         ret = imx219_get_regulators(imx219);
1541         if (ret) {
1542                 dev_err(dev, "failed to get regulators\n");
1543                 return ret;
1544         }
1545
1546         ret = imx219_sensor_pinctrl_init(&imx219->imx219_pctrl, dev);
1547         if (ret) {
1548                 pr_err("Can't get pinctrl, use gpio to ctrl\n");
1549                 /* Request optional enable pin */
1550                 imx219->reset_gpio = devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_HIGH);
1551                 imx219->imx219_pctrl.use_pinctrl = false;
1552         }
1553
1554         /*
1555          * The sensor must be powered for imx219_identify_module()
1556          * to be able to read the CHIP_ID register
1557          */
1558         ret = imx219_power_on(dev);
1559         if (ret)
1560                 return ret;
1561
1562         ret = imx219_identify_module(imx219);
1563         if (ret)
1564                 goto error_power_off;
1565
1566         /* Set default mode to max resolution */
1567         imx219->mode = &supported_modes[0];
1568         imx219->frame_interval.numerator = 1;
1569         imx219->frame_interval.denominator = supported_modes[0].fps;
1570
1571         /* sensor doesn't enter LP-11 state upon power up until and unless
1572          * streaming is started, so upon power up switch the modes to:
1573          * streaming -> standby
1574          */
1575         ret = imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
1576                         IMX219_REG_VALUE_08BIT, IMX219_MODE_STREAMING);
1577         if (ret < 0)
1578                 goto error_power_off;
1579         usleep_range(100, 110);
1580
1581         /* put sensor back to standby mode */
1582         ret = imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
1583                         IMX219_REG_VALUE_08BIT, IMX219_MODE_STANDBY);
1584         if (ret < 0)
1585                 goto error_power_off;
1586         usleep_range(100, 110);
1587
1588         ret = imx219_init_controls(imx219);
1589         if (ret)
1590                 goto error_power_off;
1591
1592         /* Initialize subdev */
1593         imx219->sd.internal_ops = &imx219_internal_ops;
1594         imx219->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
1595         imx219->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1596
1597         /* Initialize source pad */
1598         imx219->pad.flags = MEDIA_PAD_FL_SOURCE;
1599
1600         /* Initialize default format */
1601         imx219_set_default_format(imx219);
1602
1603         ret = media_entity_pads_init(&imx219->sd.entity, 1, &imx219->pad);
1604         if (ret) {
1605                 dev_err(dev, "failed to init entity pads: %d\n", ret);
1606                 goto error_handler_free;
1607         }
1608
1609         ret = v4l2_async_register_subdev_sensor(&imx219->sd);
1610         if (ret < 0) {
1611                 dev_err(dev, "failed to register sensor sub-device: %d\n", ret);
1612                 goto error_media_entity;
1613         }
1614
1615         /* Enable runtime PM and turn off the device */
1616         pm_runtime_set_active(dev);
1617         pm_runtime_enable(dev);
1618         pm_runtime_idle(dev);
1619
1620         return 0;
1621
1622 error_media_entity:
1623         media_entity_cleanup(&imx219->sd.entity);
1624
1625 error_handler_free:
1626         imx219_free_controls(imx219);
1627
1628 error_power_off:
1629         imx219_power_off(dev);
1630
1631         return ret;
1632 }
1633
1634 static int imx219_remove(struct i2c_client *client)
1635 {
1636         struct v4l2_subdev *sd = i2c_get_clientdata(client);
1637         struct imx219 *imx219 = to_imx219(sd);
1638
1639         v4l2_async_unregister_subdev(sd);
1640         media_entity_cleanup(&sd->entity);
1641         imx219_free_controls(imx219);
1642
1643         pm_runtime_disable(&client->dev);
1644         if (!pm_runtime_status_suspended(&client->dev))
1645                 imx219_power_off(&client->dev);
1646         pm_runtime_set_suspended(&client->dev);
1647
1648         return 0;
1649 }
1650
1651 static const struct of_device_id imx219_dt_ids[] = {
1652         { .compatible = "sony,imx219" },
1653         { /* sentinel */ }
1654 };
1655 MODULE_DEVICE_TABLE(of, imx219_dt_ids);
1656
1657 static const struct dev_pm_ops imx219_pm_ops = {
1658         SET_SYSTEM_SLEEP_PM_OPS(imx219_suspend, imx219_resume)
1659         SET_RUNTIME_PM_OPS(imx219_power_off, imx219_power_on, NULL)
1660 };
1661
1662 static struct i2c_driver imx219_i2c_driver = {
1663         .driver = {
1664                 .name = "imx219",
1665                 .of_match_table = imx219_dt_ids,
1666                 .pm = &imx219_pm_ops,
1667         },
1668         .probe_new = imx219_probe,
1669         .remove = imx219_remove,
1670 };
1671
1672 module_i2c_driver(imx219_i2c_driver);
1673
1674 MODULE_AUTHOR("David.li");
1675 MODULE_DESCRIPTION("Sony IMX219 sensor driver");
1676 MODULE_LICENSE("GPL v2");