target/linux/bcm27xx/patches-5.4/950-0463-media-i2c-Add-driver-for-Sony-IMX219-sensor.patch

   1 From 5cd8c4efeb46ce1ef370dd3012a7951ba430b58f Mon Sep 17 00:00:00 2001
   2 From: Dave Stevenson <dave.stevenson@raspberrypi.com>
   3 Date: Mon, 20 Jan 2020 05:15:58 -0300
   4 Subject: [PATCH] media: i2c: Add driver for Sony IMX219 sensor
   5
   6 Commit 1283b3b8f82b9004fbb94398cade5c8e797a2c8d upstream.
   7 (Currently on linux-media/master, queued for 5.7)
   8
   9 Adds a driver for the 8MPix Sony IMX219 CSI2 sensor.
  10 Whilst the sensor supports 2 or 4 CSI2 data lanes, this driver
  11 currently only supports 2 lanes.
  12 8MPix @ 15fps, 1080P @ 30fps (cropped FOV), and 1640x1232 (2x2 binned)
  13 @ 30fps are currently supported.
  14
  15 [Sakari Ailus: make imx219_check_hwcfg static]
  16
  17 Signed-off-by: Dave Stevenson <dave.stevenson@raspberrypi.com>
  18 Signed-off-by: Andrey Konovalov <andrey.konovalov@linaro.org>
  19 Signed-off-by: Sakari Ailus <sakari.ailus@linux.intel.com>
  20 Signed-off-by: Mauro Carvalho Chehab <mchehab+samsung@kernel.org>
  21 ---
  22  drivers/media/i2c/Kconfig  |   11 +
  23  drivers/media/i2c/Makefile |    1 +
  24  drivers/media/i2c/imx219.c | 1312 ++++++++++++++++++++++++++++++++++++
  25  3 files changed, 1324 insertions(+)
  26  create mode 100644 drivers/media/i2c/imx219.c
  27
  28 --- a/drivers/media/i2c/Kconfig
  29 +++ b/drivers/media/i2c/Kconfig
  30 @@ -578,6 +578,17 @@ config VIDEO_IMX214
  31           To compile this driver as a module, choose M here: the
  32           module will be called imx214.
  33
  34 +config VIDEO_IMX219
  35 +       tristate "Sony IMX219 sensor support"
  36 +       depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
  37 +       select V4L2_FWNODE
  38 +       help
  39 +         This is a Video4Linux2 sensor driver for the Sony
  40 +         IMX219 camera.
  41 +
  42 +         To compile this driver as a module, choose M here: the
  43 +         module will be called imx219.
  44 +
  45  config VIDEO_IMX258
  46         tristate "Sony IMX258 sensor support"
  47         depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
  48 --- a/drivers/media/i2c/Makefile
  49 +++ b/drivers/media/i2c/Makefile
  50 @@ -111,6 +111,7 @@ obj-$(CONFIG_VIDEO_ML86V7667)       += ml86v76
  51  obj-$(CONFIG_VIDEO_OV2659)     += ov2659.o
  52  obj-$(CONFIG_VIDEO_TC358743)   += tc358743.o
  53  obj-$(CONFIG_VIDEO_IMX214)     += imx214.o
  54 +obj-$(CONFIG_VIDEO_IMX219)     += imx219.o
  55  obj-$(CONFIG_VIDEO_IMX258)     += imx258.o
  56  obj-$(CONFIG_VIDEO_IMX274)     += imx274.o
  57  obj-$(CONFIG_VIDEO_IMX319)     += imx319.o
  58 --- /dev/null
  59 +++ b/drivers/media/i2c/imx219.c
  60 @@ -0,0 +1,1312 @@
  61 +// SPDX-License-Identifier: GPL-2.0
  62 +/*
  63 + * A V4L2 driver for Sony IMX219 cameras.
  64 + * Copyright (C) 2019, Raspberry Pi (Trading) Ltd
  65 + *
  66 + * Based on Sony imx258 camera driver
  67 + * Copyright (C) 2018 Intel Corporation
  68 + *
  69 + * DT / fwnode changes, and regulator / GPIO control taken from imx214 driver
  70 + * Copyright 2018 Qtechnology A/S
  71 + *
  72 + * Flip handling taken from the Sony IMX319 driver.
  73 + * Copyright (C) 2018 Intel Corporation
  74 + *
  75 + */
  76 +
  77 +#include <linux/clk.h>
  78 +#include <linux/clk-provider.h>
  79 +#include <linux/clkdev.h>
  80 +#include <linux/delay.h>
  81 +#include <linux/gpio/consumer.h>
  82 +#include <linux/i2c.h>
  83 +#include <linux/module.h>
  84 +#include <linux/pm_runtime.h>
  85 +#include <linux/regulator/consumer.h>
  86 +#include <media/v4l2-ctrls.h>
  87 +#include <media/v4l2-device.h>
  88 +#include <media/v4l2-event.h>
  89 +#include <media/v4l2-fwnode.h>
  90 +#include <media/v4l2-mediabus.h>
  91 +#include <asm/unaligned.h>
  92 +
  93 +#define IMX219_REG_VALUE_08BIT         1
  94 +#define IMX219_REG_VALUE_16BIT         2
  95 +
  96 +#define IMX219_REG_MODE_SELECT         0x0100
  97 +#define IMX219_MODE_STANDBY            0x00
  98 +#define IMX219_MODE_STREAMING          0x01
  99 +
 100 +/* Chip ID */
 101 +#define IMX219_REG_CHIP_ID             0x0000
 102 +#define IMX219_CHIP_ID                 0x0219
 103 +
 104 +/* External clock frequency is 24.0M */
 105 +#define IMX219_XCLK_FREQ               24000000
 106 +
 107 +/* Pixel rate is fixed at 182.4M for all the modes */
 108 +#define IMX219_PIXEL_RATE              182400000
 109 +
 110 +#define IMX219_DEFAULT_LINK_FREQ       456000000
 111 +
 112 +/* V_TIMING internal */
 113 +#define IMX219_REG_VTS                 0x0160
 114 +#define IMX219_VTS_15FPS               0x0dc6
 115 +#define IMX219_VTS_30FPS_1080P         0x06e3
 116 +#define IMX219_VTS_30FPS_BINNED                0x06e3
 117 +#define IMX219_VTS_MAX                 0xffff
 118 +
 119 +#define IMX219_VBLANK_MIN              4
 120 +
 121 +/*Frame Length Line*/
 122 +#define IMX219_FLL_MIN                 0x08a6
 123 +#define IMX219_FLL_MAX                 0xffff
 124 +#define IMX219_FLL_STEP                        1
 125 +#define IMX219_FLL_DEFAULT             0x0c98
 126 +
 127 +/* HBLANK control - read only */
 128 +#define IMX219_PPL_DEFAULT             3448
 129 +
 130 +/* Exposure control */
 131 +#define IMX219_REG_EXPOSURE            0x015a
 132 +#define IMX219_EXPOSURE_MIN            4
 133 +#define IMX219_EXPOSURE_STEP           1
 134 +#define IMX219_EXPOSURE_DEFAULT                0x640
 135 +#define IMX219_EXPOSURE_MAX            65535
 136 +
 137 +/* Analog gain control */
 138 +#define IMX219_REG_ANALOG_GAIN         0x0157
 139 +#define IMX219_ANA_GAIN_MIN            0
 140 +#define IMX219_ANA_GAIN_MAX            232
 141 +#define IMX219_ANA_GAIN_STEP           1
 142 +#define IMX219_ANA_GAIN_DEFAULT                0x0
 143 +
 144 +/* Digital gain control */
 145 +#define IMX219_REG_DIGITAL_GAIN                0x0158
 146 +#define IMX219_DGTL_GAIN_MIN           0x0100
 147 +#define IMX219_DGTL_GAIN_MAX           0x0fff
 148 +#define IMX219_DGTL_GAIN_DEFAULT       0x0100
 149 +#define IMX219_DGTL_GAIN_STEP          1
 150 +
 151 +#define IMX219_REG_ORIENTATION         0x0172
 152 +
 153 +/* Test Pattern Control */
 154 +#define IMX219_REG_TEST_PATTERN                0x0600
 155 +#define IMX219_TEST_PATTERN_DISABLE    0
 156 +#define IMX219_TEST_PATTERN_SOLID_COLOR        1
 157 +#define IMX219_TEST_PATTERN_COLOR_BARS 2
 158 +#define IMX219_TEST_PATTERN_GREY_COLOR 3
 159 +#define IMX219_TEST_PATTERN_PN9                4
 160 +
 161 +/* Test pattern colour components */
 162 +#define IMX219_REG_TESTP_RED           0x0602
 163 +#define IMX219_REG_TESTP_GREENR                0x0604
 164 +#define IMX219_REG_TESTP_BLUE          0x0606
 165 +#define IMX219_REG_TESTP_GREENB                0x0608
 166 +#define IMX219_TESTP_COLOUR_MIN                0
 167 +#define IMX219_TESTP_COLOUR_MAX                0x03ff
 168 +#define IMX219_TESTP_COLOUR_STEP       1
 169 +#define IMX219_TESTP_RED_DEFAULT       IMX219_TESTP_COLOUR_MAX
 170 +#define IMX219_TESTP_GREENR_DEFAULT    0
 171 +#define IMX219_TESTP_BLUE_DEFAULT      0
 172 +#define IMX219_TESTP_GREENB_DEFAULT    0
 173 +
 174 +struct imx219_reg {
 175 +       u16 address;
 176 +       u8 val;
 177 +};
 178 +
 179 +struct imx219_reg_list {
 180 +       unsigned int num_of_regs;
 181 +       const struct imx219_reg *regs;
 182 +};
 183 +
 184 +/* Mode : resolution and related config&values */
 185 +struct imx219_mode {
 186 +       /* Frame width */
 187 +       unsigned int width;
 188 +       /* Frame height */
 189 +       unsigned int height;
 190 +
 191 +       /* V-timing */
 192 +       unsigned int vts_def;
 193 +
 194 +       /* Default register values */
 195 +       struct imx219_reg_list reg_list;
 196 +};
 197 +
 198 +/*
 199 + * Register sets lifted off the i2C interface from the Raspberry Pi firmware
 200 + * driver.
 201 + * 3280x2464 = mode 2, 1920x1080 = mode 1, and 1640x1232 = mode 4.
 202 + */
 203 +static const struct imx219_reg mode_3280x2464_regs[] = {
 204 +       {0x0100, 0x00},
 205 +       {0x30eb, 0x0c},
 206 +       {0x30eb, 0x05},
 207 +       {0x300a, 0xff},
 208 +       {0x300b, 0xff},
 209 +       {0x30eb, 0x05},
 210 +       {0x30eb, 0x09},
 211 +       {0x0114, 0x01},
 212 +       {0x0128, 0x00},
 213 +       {0x012a, 0x18},
 214 +       {0x012b, 0x00},
 215 +       {0x0164, 0x00},
 216 +       {0x0165, 0x00},
 217 +       {0x0166, 0x0c},
 218 +       {0x0167, 0xcf},
 219 +       {0x0168, 0x00},
 220 +       {0x0169, 0x00},
 221 +       {0x016a, 0x09},
 222 +       {0x016b, 0x9f},
 223 +       {0x016c, 0x0c},
 224 +       {0x016d, 0xd0},
 225 +       {0x016e, 0x09},
 226 +       {0x016f, 0xa0},
 227 +       {0x0170, 0x01},
 228 +       {0x0171, 0x01},
 229 +       {0x0174, 0x00},
 230 +       {0x0175, 0x00},
 231 +       {0x018c, 0x0a},
 232 +       {0x018d, 0x0a},
 233 +       {0x0301, 0x05},
 234 +       {0x0303, 0x01},
 235 +       {0x0304, 0x03},
 236 +       {0x0305, 0x03},
 237 +       {0x0306, 0x00},
 238 +       {0x0307, 0x39},
 239 +       {0x0309, 0x0a},
 240 +       {0x030b, 0x01},
 241 +       {0x030c, 0x00},
 242 +       {0x030d, 0x72},
 243 +       {0x0624, 0x0c},
 244 +       {0x0625, 0xd0},
 245 +       {0x0626, 0x09},
 246 +       {0x0627, 0xa0},
 247 +       {0x455e, 0x00},
 248 +       {0x471e, 0x4b},
 249 +       {0x4767, 0x0f},
 250 +       {0x4750, 0x14},
 251 +       {0x4540, 0x00},
 252 +       {0x47b4, 0x14},
 253 +       {0x4713, 0x30},
 254 +       {0x478b, 0x10},
 255 +       {0x478f, 0x10},
 256 +       {0x4793, 0x10},
 257 +       {0x4797, 0x0e},
 258 +       {0x479b, 0x0e},
 259 +       {0x0162, 0x0d},
 260 +       {0x0163, 0x78},
 261 +};
 262 +
 263 +static const struct imx219_reg mode_1920_1080_regs[] = {
 264 +       {0x0100, 0x00},
 265 +       {0x30eb, 0x05},
 266 +       {0x30eb, 0x0c},
 267 +       {0x300a, 0xff},
 268 +       {0x300b, 0xff},
 269 +       {0x30eb, 0x05},
 270 +       {0x30eb, 0x09},
 271 +       {0x0114, 0x01},
 272 +       {0x0128, 0x00},
 273 +       {0x012a, 0x18},
 274 +       {0x012b, 0x00},
 275 +       {0x0162, 0x0d},
 276 +       {0x0163, 0x78},
 277 +       {0x0164, 0x02},
 278 +       {0x0165, 0xa8},
 279 +       {0x0166, 0x0a},
 280 +       {0x0167, 0x27},
 281 +       {0x0168, 0x02},
 282 +       {0x0169, 0xb4},
 283 +       {0x016a, 0x06},
 284 +       {0x016b, 0xeb},
 285 +       {0x016c, 0x07},
 286 +       {0x016d, 0x80},
 287 +       {0x016e, 0x04},
 288 +       {0x016f, 0x38},
 289 +       {0x0170, 0x01},
 290 +       {0x0171, 0x01},
 291 +       {0x0174, 0x00},
 292 +       {0x0175, 0x00},
 293 +       {0x018c, 0x0a},
 294 +       {0x018d, 0x0a},
 295 +       {0x0301, 0x05},
 296 +       {0x0303, 0x01},
 297 +       {0x0304, 0x03},
 298 +       {0x0305, 0x03},
 299 +       {0x0306, 0x00},
 300 +       {0x0307, 0x39},
 301 +       {0x0309, 0x0a},
 302 +       {0x030b, 0x01},
 303 +       {0x030c, 0x00},
 304 +       {0x030d, 0x72},
 305 +       {0x0624, 0x07},
 306 +       {0x0625, 0x80},
 307 +       {0x0626, 0x04},
 308 +       {0x0627, 0x38},
 309 +       {0x455e, 0x00},
 310 +       {0x471e, 0x4b},
 311 +       {0x4767, 0x0f},
 312 +       {0x4750, 0x14},
 313 +       {0x4540, 0x00},
 314 +       {0x47b4, 0x14},
 315 +       {0x4713, 0x30},
 316 +       {0x478b, 0x10},
 317 +       {0x478f, 0x10},
 318 +       {0x4793, 0x10},
 319 +       {0x4797, 0x0e},
 320 +       {0x479b, 0x0e},
 321 +       {0x0162, 0x0d},
 322 +       {0x0163, 0x78},
 323 +};
 324 +
 325 +static const struct imx219_reg mode_1640_1232_regs[] = {
 326 +       {0x0100, 0x00},
 327 +       {0x30eb, 0x0c},
 328 +       {0x30eb, 0x05},
 329 +       {0x300a, 0xff},
 330 +       {0x300b, 0xff},
 331 +       {0x30eb, 0x05},
 332 +       {0x30eb, 0x09},
 333 +       {0x0114, 0x01},
 334 +       {0x0128, 0x00},
 335 +       {0x012a, 0x18},
 336 +       {0x012b, 0x00},
 337 +       {0x0164, 0x00},
 338 +       {0x0165, 0x00},
 339 +       {0x0166, 0x0c},
 340 +       {0x0167, 0xcf},
 341 +       {0x0168, 0x00},
 342 +       {0x0169, 0x00},
 343 +       {0x016a, 0x09},
 344 +       {0x016b, 0x9f},
 345 +       {0x016c, 0x06},
 346 +       {0x016d, 0x68},
 347 +       {0x016e, 0x04},
 348 +       {0x016f, 0xd0},
 349 +       {0x0170, 0x01},
 350 +       {0x0171, 0x01},
 351 +       {0x0174, 0x01},
 352 +       {0x0175, 0x01},
 353 +       {0x018c, 0x0a},
 354 +       {0x018d, 0x0a},
 355 +       {0x0301, 0x05},
 356 +       {0x0303, 0x01},
 357 +       {0x0304, 0x03},
 358 +       {0x0305, 0x03},
 359 +       {0x0306, 0x00},
 360 +       {0x0307, 0x39},
 361 +       {0x0309, 0x0a},
 362 +       {0x030b, 0x01},
 363 +       {0x030c, 0x00},
 364 +       {0x030d, 0x72},
 365 +       {0x0624, 0x06},
 366 +       {0x0625, 0x68},
 367 +       {0x0626, 0x04},
 368 +       {0x0627, 0xd0},
 369 +       {0x455e, 0x00},
 370 +       {0x471e, 0x4b},
 371 +       {0x4767, 0x0f},
 372 +       {0x4750, 0x14},
 373 +       {0x4540, 0x00},
 374 +       {0x47b4, 0x14},
 375 +       {0x4713, 0x30},
 376 +       {0x478b, 0x10},
 377 +       {0x478f, 0x10},
 378 +       {0x4793, 0x10},
 379 +       {0x4797, 0x0e},
 380 +       {0x479b, 0x0e},
 381 +       {0x0162, 0x0d},
 382 +       {0x0163, 0x78},
 383 +};
 384 +
 385 +static const char * const imx219_test_pattern_menu[] = {
 386 +       "Disabled",
 387 +       "Color Bars",
 388 +       "Solid Color",
 389 +       "Grey Color Bars",
 390 +       "PN9"
 391 +};
 392 +
 393 +static const int imx219_test_pattern_val[] = {
 394 +       IMX219_TEST_PATTERN_DISABLE,
 395 +       IMX219_TEST_PATTERN_COLOR_BARS,
 396 +       IMX219_TEST_PATTERN_SOLID_COLOR,
 397 +       IMX219_TEST_PATTERN_GREY_COLOR,
 398 +       IMX219_TEST_PATTERN_PN9,
 399 +};
 400 +
 401 +/* regulator supplies */
 402 +static const char * const imx219_supply_name[] = {
 403 +       /* Supplies can be enabled in any order */
 404 +       "VANA",  /* Analog (2.8V) supply */
 405 +       "VDIG",  /* Digital Core (1.8V) supply */
 406 +       "VDDL",  /* IF (1.2V) supply */
 407 +};
 408 +
 409 +#define IMX219_NUM_SUPPLIES ARRAY_SIZE(imx219_supply_name)
 410 +
 411 +/*
 412 + * Initialisation delay between XCLR low->high and the moment when the sensor
 413 + * can start capture (i.e. can leave software stanby) must be not less than:
 414 + *   t4 + max(t5, t6 + <time to initialize the sensor register over I2C>)
 415 + * where
 416 + *   t4 is fixed, and is max 200uS,
 417 + *   t5 is fixed, and is 6000uS,
 418 + *   t6 depends on the sensor external clock, and is max 32000 clock periods.
 419 + * As per sensor datasheet, the external clock must be from 6MHz to 27MHz.
 420 + * So for any acceptable external clock t6 is always within the range of
 421 + * 1185 to 5333 uS, and is always less than t5.
 422 + * For this reason this is always safe to wait (t4 + t5) = 6200 uS, then
 423 + * initialize the sensor over I2C, and then exit the software standby.
 424 + *
 425 + * This start-up time can be optimized a bit more, if we start the writes
 426 + * over I2C after (t4+t6), but before (t4+t5) expires. But then sensor
 427 + * initialization over I2C may complete before (t4+t5) expires, and we must
 428 + * ensure that capture is not started before (t4+t5).
 429 + *
 430 + * This delay doesn't account for the power supply startup time. If needed,
 431 + * this should be taken care of via the regulator framework. E.g. in the
 432 + * case of DT for regulator-fixed one should define the startup-delay-us
 433 + * property.
 434 + */
 435 +#define IMX219_XCLR_MIN_DELAY_US       6200
 436 +#define IMX219_XCLR_DELAY_RANGE_US     1000
 437 +
 438 +/* Mode configs */
 439 +static const struct imx219_mode supported_modes[] = {
 440 +       {
 441 +               /* 8MPix 15fps mode */
 442 +               .width = 3280,
 443 +               .height = 2464,
 444 +               .vts_def = IMX219_VTS_15FPS,
 445 +               .reg_list = {
 446 +                       .num_of_regs = ARRAY_SIZE(mode_3280x2464_regs),
 447 +                       .regs = mode_3280x2464_regs,
 448 +               },
 449 +       },
 450 +       {
 451 +               /* 1080P 30fps cropped */
 452 +               .width = 1920,
 453 +               .height = 1080,
 454 +               .vts_def = IMX219_VTS_30FPS_1080P,
 455 +               .reg_list = {
 456 +                       .num_of_regs = ARRAY_SIZE(mode_1920_1080_regs),
 457 +                       .regs = mode_1920_1080_regs,
 458 +               },
 459 +       },
 460 +       {
 461 +               /* 2x2 binned 30fps mode */
 462 +               .width = 1640,
 463 +               .height = 1232,
 464 +               .vts_def = IMX219_VTS_30FPS_BINNED,
 465 +               .reg_list = {
 466 +                       .num_of_regs = ARRAY_SIZE(mode_1640_1232_regs),
 467 +                       .regs = mode_1640_1232_regs,
 468 +               },
 469 +       },
 470 +};
 471 +
 472 +struct imx219 {
 473 +       struct v4l2_subdev sd;
 474 +       struct media_pad pad;
 475 +
 476 +       struct clk *xclk; /* system clock to IMX219 */
 477 +       u32 xclk_freq;
 478 +
 479 +       struct gpio_desc *reset_gpio;
 480 +       struct regulator_bulk_data supplies[IMX219_NUM_SUPPLIES];
 481 +
 482 +       struct v4l2_ctrl_handler ctrl_handler;
 483 +       /* V4L2 Controls */
 484 +       struct v4l2_ctrl *pixel_rate;
 485 +       struct v4l2_ctrl *exposure;
 486 +       struct v4l2_ctrl *vflip;
 487 +       struct v4l2_ctrl *hflip;
 488 +       struct v4l2_ctrl *vblank;
 489 +       struct v4l2_ctrl *hblank;
 490 +
 491 +       /* Current mode */
 492 +       const struct imx219_mode *mode;
 493 +
 494 +       /*
 495 +        * Mutex for serialized access:
 496 +        * Protect sensor module set pad format and start/stop streaming safely.
 497 +        */
 498 +       struct mutex mutex;
 499 +
 500 +       /* Streaming on/off */
 501 +       bool streaming;
 502 +};
 503 +
 504 +static inline struct imx219 *to_imx219(struct v4l2_subdev *_sd)
 505 +{
 506 +       return container_of(_sd, struct imx219, sd);
 507 +}
 508 +
 509 +/* Read registers up to 2 at a time */
 510 +static int imx219_read_reg(struct imx219 *imx219, u16 reg, u32 len, u32 *val)
 511 +{
 512 +       struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 513 +       struct i2c_msg msgs[2];
 514 +       u8 addr_buf[2] = { reg >> 8, reg & 0xff };
 515 +       u8 data_buf[4] = { 0, };
 516 +       int ret;
 517 +
 518 +       if (len > 4)
 519 +               return -EINVAL;
 520 +
 521 +       /* Write register address */
 522 +       msgs[0].addr = client->addr;
 523 +       msgs[0].flags = 0;
 524 +       msgs[0].len = ARRAY_SIZE(addr_buf);
 525 +       msgs[0].buf = addr_buf;
 526 +
 527 +       /* Read data from register */
 528 +       msgs[1].addr = client->addr;
 529 +       msgs[1].flags = I2C_M_RD;
 530 +       msgs[1].len = len;
 531 +       msgs[1].buf = &data_buf[4 - len];
 532 +
 533 +       ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
 534 +       if (ret != ARRAY_SIZE(msgs))
 535 +               return -EIO;
 536 +
 537 +       *val = get_unaligned_be32(data_buf);
 538 +
 539 +       return 0;
 540 +}
 541 +
 542 +/* Write registers up to 2 at a time */
 543 +static int imx219_write_reg(struct imx219 *imx219, u16 reg, u32 len, u32 val)
 544 +{
 545 +       struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 546 +       u8 buf[6];
 547 +
 548 +       if (len > 4)
 549 +               return -EINVAL;
 550 +
 551 +       put_unaligned_be16(reg, buf);
 552 +       put_unaligned_be32(val << (8 * (4 - len)), buf + 2);
 553 +       if (i2c_master_send(client, buf, len + 2) != len + 2)
 554 +               return -EIO;
 555 +
 556 +       return 0;
 557 +}
 558 +
 559 +/* Write a list of registers */
 560 +static int imx219_write_regs(struct imx219 *imx219,
 561 +                            const struct imx219_reg *regs, u32 len)
 562 +{
 563 +       struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 564 +       unsigned int i;
 565 +       int ret;
 566 +
 567 +       for (i = 0; i < len; i++) {
 568 +               ret = imx219_write_reg(imx219, regs[i].address, 1, regs[i].val);
 569 +               if (ret) {
 570 +                       dev_err_ratelimited(&client->dev,
 571 +                                           "Failed to write reg 0x%4.4x. error = %d\n",
 572 +                                           regs[i].address, ret);
 573 +
 574 +                       return ret;
 575 +               }
 576 +       }
 577 +
 578 +       return 0;
 579 +}
 580 +
 581 +/* Get bayer order based on flip setting. */
 582 +static u32 imx219_get_format_code(struct imx219 *imx219)
 583 +{
 584 +       /*
 585 +        * Only one bayer order is supported.
 586 +        * It depends on the flip settings.
 587 +        */
 588 +       static const u32 codes[2][2] = {
 589 +               { MEDIA_BUS_FMT_SRGGB10_1X10, MEDIA_BUS_FMT_SGRBG10_1X10, },
 590 +               { MEDIA_BUS_FMT_SGBRG10_1X10, MEDIA_BUS_FMT_SBGGR10_1X10, },
 591 +       };
 592 +
 593 +       lockdep_assert_held(&imx219->mutex);
 594 +       return codes[imx219->vflip->val][imx219->hflip->val];
 595 +}
 596 +
 597 +static int imx219_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
 598 +{
 599 +       struct imx219 *imx219 = to_imx219(sd);
 600 +       struct v4l2_mbus_framefmt *try_fmt =
 601 +               v4l2_subdev_get_try_format(sd, fh->pad, 0);
 602 +
 603 +       mutex_lock(&imx219->mutex);
 604 +
 605 +       /* Initialize try_fmt */
 606 +       try_fmt->width = supported_modes[0].width;
 607 +       try_fmt->height = supported_modes[0].height;
 608 +       try_fmt->code = imx219_get_format_code(imx219);
 609 +       try_fmt->field = V4L2_FIELD_NONE;
 610 +
 611 +       mutex_unlock(&imx219->mutex);
 612 +
 613 +       return 0;
 614 +}
 615 +
 616 +static int imx219_set_ctrl(struct v4l2_ctrl *ctrl)
 617 +{
 618 +       struct imx219 *imx219 =
 619 +               container_of(ctrl->handler, struct imx219, ctrl_handler);
 620 +       struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 621 +       int ret;
 622 +
 623 +       if (ctrl->id == V4L2_CID_VBLANK) {
 624 +               int exposure_max, exposure_def;
 625 +
 626 +               /* Update max exposure while meeting expected vblanking */
 627 +               exposure_max = imx219->mode->height + ctrl->val - 4;
 628 +               exposure_def = (exposure_max < IMX219_EXPOSURE_DEFAULT) ?
 629 +                       exposure_max : IMX219_EXPOSURE_DEFAULT;
 630 +               __v4l2_ctrl_modify_range(imx219->exposure,
 631 +                                        imx219->exposure->minimum,
 632 +                                        exposure_max, imx219->exposure->step,
 633 +                                        exposure_def);
 634 +       }
 635 +
 636 +       /*
 637 +        * Applying V4L2 control value only happens
 638 +        * when power is up for streaming
 639 +        */
 640 +       if (pm_runtime_get_if_in_use(&client->dev) == 0)
 641 +               return 0;
 642 +
 643 +       switch (ctrl->id) {
 644 +       case V4L2_CID_ANALOGUE_GAIN:
 645 +               ret = imx219_write_reg(imx219, IMX219_REG_ANALOG_GAIN,
 646 +                                      IMX219_REG_VALUE_08BIT, ctrl->val);
 647 +               break;
 648 +       case V4L2_CID_EXPOSURE:
 649 +               ret = imx219_write_reg(imx219, IMX219_REG_EXPOSURE,
 650 +                                      IMX219_REG_VALUE_16BIT, ctrl->val);
 651 +               break;
 652 +       case V4L2_CID_DIGITAL_GAIN:
 653 +               ret = imx219_write_reg(imx219, IMX219_REG_DIGITAL_GAIN,
 654 +                                      IMX219_REG_VALUE_16BIT, ctrl->val);
 655 +               break;
 656 +       case V4L2_CID_TEST_PATTERN:
 657 +               ret = imx219_write_reg(imx219, IMX219_REG_TEST_PATTERN,
 658 +                                      IMX219_REG_VALUE_16BIT,
 659 +                                      imx219_test_pattern_val[ctrl->val]);
 660 +               break;
 661 +       case V4L2_CID_HFLIP:
 662 +       case V4L2_CID_VFLIP:
 663 +               ret = imx219_write_reg(imx219, IMX219_REG_ORIENTATION, 1,
 664 +                                      imx219->hflip->val |
 665 +                                      imx219->vflip->val << 1);
 666 +               break;
 667 +       case V4L2_CID_VBLANK:
 668 +               ret = imx219_write_reg(imx219, IMX219_REG_VTS,
 669 +                                      IMX219_REG_VALUE_16BIT,
 670 +                                      imx219->mode->height + ctrl->val);
 671 +               break;
 672 +       case V4L2_CID_TEST_PATTERN_RED:
 673 +               ret = imx219_write_reg(imx219, IMX219_REG_TESTP_RED,
 674 +                                      IMX219_REG_VALUE_16BIT, ctrl->val);
 675 +               break;
 676 +       case V4L2_CID_TEST_PATTERN_GREENR:
 677 +               ret = imx219_write_reg(imx219, IMX219_REG_TESTP_GREENR,
 678 +                                      IMX219_REG_VALUE_16BIT, ctrl->val);
 679 +               break;
 680 +       case V4L2_CID_TEST_PATTERN_BLUE:
 681 +               ret = imx219_write_reg(imx219, IMX219_REG_TESTP_BLUE,
 682 +                                      IMX219_REG_VALUE_16BIT, ctrl->val);
 683 +               break;
 684 +       case V4L2_CID_TEST_PATTERN_GREENB:
 685 +               ret = imx219_write_reg(imx219, IMX219_REG_TESTP_GREENB,
 686 +                                      IMX219_REG_VALUE_16BIT, ctrl->val);
 687 +               break;
 688 +       default:
 689 +               dev_info(&client->dev,
 690 +                        "ctrl(id:0x%x,val:0x%x) is not handled\n",
 691 +                        ctrl->id, ctrl->val);
 692 +               ret = -EINVAL;
 693 +               break;
 694 +       }
 695 +
 696 +       pm_runtime_put(&client->dev);
 697 +
 698 +       return ret;
 699 +}
 700 +
 701 +static const struct v4l2_ctrl_ops imx219_ctrl_ops = {
 702 +       .s_ctrl = imx219_set_ctrl,
 703 +};
 704 +
 705 +static int imx219_enum_mbus_code(struct v4l2_subdev *sd,
 706 +                                struct v4l2_subdev_pad_config *cfg,
 707 +                                struct v4l2_subdev_mbus_code_enum *code)
 708 +{
 709 +       struct imx219 *imx219 = to_imx219(sd);
 710 +
 711 +       /*
 712 +        * Only one bayer order is supported (though it depends on the flip
 713 +        * settings)
 714 +        */
 715 +       if (code->index > 0)
 716 +               return -EINVAL;
 717 +
 718 +       code->code = imx219_get_format_code(imx219);
 719 +
 720 +       return 0;
 721 +}
 722 +
 723 +static int imx219_enum_frame_size(struct v4l2_subdev *sd,
 724 +                                 struct v4l2_subdev_pad_config *cfg,
 725 +                                 struct v4l2_subdev_frame_size_enum *fse)
 726 +{
 727 +       struct imx219 *imx219 = to_imx219(sd);
 728 +
 729 +       if (fse->index >= ARRAY_SIZE(supported_modes))
 730 +               return -EINVAL;
 731 +
 732 +       if (fse->code != imx219_get_format_code(imx219))
 733 +               return -EINVAL;
 734 +
 735 +       fse->min_width = supported_modes[fse->index].width;
 736 +       fse->max_width = fse->min_width;
 737 +       fse->min_height = supported_modes[fse->index].height;
 738 +       fse->max_height = fse->min_height;
 739 +
 740 +       return 0;
 741 +}
 742 +
 743 +static void imx219_reset_colorspace(struct v4l2_mbus_framefmt *fmt)
 744 +{
 745 +       fmt->colorspace = V4L2_COLORSPACE_SRGB;
 746 +       fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
 747 +       fmt->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
 748 +                                                         fmt->colorspace,
 749 +                                                         fmt->ycbcr_enc);
 750 +       fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
 751 +}
 752 +
 753 +static void imx219_update_pad_format(struct imx219 *imx219,
 754 +                                    const struct imx219_mode *mode,
 755 +                                    struct v4l2_subdev_format *fmt)
 756 +{
 757 +       fmt->format.width = mode->width;
 758 +       fmt->format.height = mode->height;
 759 +       fmt->format.code = imx219_get_format_code(imx219);
 760 +       fmt->format.field = V4L2_FIELD_NONE;
 761 +
 762 +       imx219_reset_colorspace(&fmt->format);
 763 +}
 764 +
 765 +static int __imx219_get_pad_format(struct imx219 *imx219,
 766 +                                  struct v4l2_subdev_pad_config *cfg,
 767 +                                  struct v4l2_subdev_format *fmt)
 768 +{
 769 +       if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
 770 +               struct v4l2_mbus_framefmt *try_fmt =
 771 +                       v4l2_subdev_get_try_format(&imx219->sd, cfg, fmt->pad);
 772 +               /* update the code which could change due to vflip or hflip: */
 773 +               try_fmt->code = imx219_get_format_code(imx219);
 774 +               fmt->format = *try_fmt;
 775 +       } else {
 776 +               imx219_update_pad_format(imx219, imx219->mode, fmt);
 777 +       }
 778 +
 779 +       return 0;
 780 +}
 781 +
 782 +static int imx219_get_pad_format(struct v4l2_subdev *sd,
 783 +                                struct v4l2_subdev_pad_config *cfg,
 784 +                                struct v4l2_subdev_format *fmt)
 785 +{
 786 +       struct imx219 *imx219 = to_imx219(sd);
 787 +       int ret;
 788 +
 789 +       mutex_lock(&imx219->mutex);
 790 +       ret = __imx219_get_pad_format(imx219, cfg, fmt);
 791 +       mutex_unlock(&imx219->mutex);
 792 +
 793 +       return ret;
 794 +}
 795 +
 796 +static int imx219_set_pad_format(struct v4l2_subdev *sd,
 797 +                                struct v4l2_subdev_pad_config *cfg,
 798 +                                struct v4l2_subdev_format *fmt)
 799 +{
 800 +       struct imx219 *imx219 = to_imx219(sd);
 801 +       const struct imx219_mode *mode;
 802 +       struct v4l2_mbus_framefmt *framefmt;
 803 +       int exposure_max, exposure_def, hblank;
 804 +
 805 +       mutex_lock(&imx219->mutex);
 806 +
 807 +       /* Bayer order varies with flips */
 808 +       fmt->format.code = imx219_get_format_code(imx219);
 809 +
 810 +       mode = v4l2_find_nearest_size(supported_modes,
 811 +                                     ARRAY_SIZE(supported_modes),
 812 +                                     width, height,
 813 +                                     fmt->format.width, fmt->format.height);
 814 +       imx219_update_pad_format(imx219, mode, fmt);
 815 +       if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
 816 +               framefmt = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
 817 +               *framefmt = fmt->format;
 818 +       } else if (imx219->mode != mode) {
 819 +               imx219->mode = mode;
 820 +               /* Update limits and set FPS to default */
 821 +               __v4l2_ctrl_modify_range(imx219->vblank, IMX219_VBLANK_MIN,
 822 +                                        IMX219_VTS_MAX - mode->height, 1,
 823 +                                        mode->vts_def - mode->height);
 824 +               __v4l2_ctrl_s_ctrl(imx219->vblank,
 825 +                                  mode->vts_def - mode->height);
 826 +               /* Update max exposure while meeting expected vblanking */
 827 +               exposure_max = mode->vts_def - 4;
 828 +               exposure_def = (exposure_max < IMX219_EXPOSURE_DEFAULT) ?
 829 +                       exposure_max : IMX219_EXPOSURE_DEFAULT;
 830 +               __v4l2_ctrl_modify_range(imx219->exposure,
 831 +                                        imx219->exposure->minimum,
 832 +                                        exposure_max, imx219->exposure->step,
 833 +                                        exposure_def);
 834 +               /*
 835 +                * Currently PPL is fixed to IMX219_PPL_DEFAULT, so hblank
 836 +                * depends on mode->width only, and is not changeble in any
 837 +                * way other than changing the mode.
 838 +                */
 839 +               hblank = IMX219_PPL_DEFAULT - mode->width;
 840 +               __v4l2_ctrl_modify_range(imx219->hblank, hblank, hblank, 1,
 841 +                                        hblank);
 842 +       }
 843 +
 844 +       mutex_unlock(&imx219->mutex);
 845 +
 846 +       return 0;
 847 +}
 848 +
 849 +static int imx219_start_streaming(struct imx219 *imx219)
 850 +{
 851 +       struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 852 +       const struct imx219_reg_list *reg_list;
 853 +       int ret;
 854 +
 855 +       /* Apply default values of current mode */
 856 +       reg_list = &imx219->mode->reg_list;
 857 +       ret = imx219_write_regs(imx219, reg_list->regs, reg_list->num_of_regs);
 858 +       if (ret) {
 859 +               dev_err(&client->dev, "%s failed to set mode\n", __func__);
 860 +               return ret;
 861 +       }
 862 +
 863 +       /* Apply customized values from user */
 864 +       ret =  __v4l2_ctrl_handler_setup(imx219->sd.ctrl_handler);
 865 +       if (ret)
 866 +               return ret;
 867 +
 868 +       /* set stream on register */
 869 +       return imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
 870 +                               IMX219_REG_VALUE_08BIT, IMX219_MODE_STREAMING);
 871 +}
 872 +
 873 +static void imx219_stop_streaming(struct imx219 *imx219)
 874 +{
 875 +       struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 876 +       int ret;
 877 +
 878 +       /* set stream off register */
 879 +       ret = imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
 880 +                              IMX219_REG_VALUE_08BIT, IMX219_MODE_STANDBY);
 881 +       if (ret)
 882 +               dev_err(&client->dev, "%s failed to set stream\n", __func__);
 883 +}
 884 +
 885 +static int imx219_set_stream(struct v4l2_subdev *sd, int enable)
 886 +{
 887 +       struct imx219 *imx219 = to_imx219(sd);
 888 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
 889 +       int ret = 0;
 890 +
 891 +       mutex_lock(&imx219->mutex);
 892 +       if (imx219->streaming == enable) {
 893 +               mutex_unlock(&imx219->mutex);
 894 +               return 0;
 895 +       }
 896 +
 897 +       if (enable) {
 898 +               ret = pm_runtime_get_sync(&client->dev);
 899 +               if (ret < 0) {
 900 +                       pm_runtime_put_noidle(&client->dev);
 901 +                       goto err_unlock;
 902 +               }
 903 +
 904 +               /*
 905 +                * Apply default & customized values
 906 +                * and then start streaming.
 907 +                */
 908 +               ret = imx219_start_streaming(imx219);
 909 +               if (ret)
 910 +                       goto err_rpm_put;
 911 +       } else {
 912 +               imx219_stop_streaming(imx219);
 913 +               pm_runtime_put(&client->dev);
 914 +       }
 915 +
 916 +       imx219->streaming = enable;
 917 +
 918 +       /* vflip and hflip cannot change during streaming */
 919 +       __v4l2_ctrl_grab(imx219->vflip, enable);
 920 +       __v4l2_ctrl_grab(imx219->hflip, enable);
 921 +
 922 +       mutex_unlock(&imx219->mutex);
 923 +
 924 +       return ret;
 925 +
 926 +err_rpm_put:
 927 +       pm_runtime_put(&client->dev);
 928 +err_unlock:
 929 +       mutex_unlock(&imx219->mutex);
 930 +
 931 +       return ret;
 932 +}
 933 +
 934 +/* Power/clock management functions */
 935 +static int imx219_power_on(struct device *dev)
 936 +{
 937 +       struct i2c_client *client = to_i2c_client(dev);
 938 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
 939 +       struct imx219 *imx219 = to_imx219(sd);
 940 +       int ret;
 941 +
 942 +       ret = regulator_bulk_enable(IMX219_NUM_SUPPLIES,
 943 +                                   imx219->supplies);
 944 +       if (ret) {
 945 +               dev_err(&client->dev, "%s: failed to enable regulators\n",
 946 +                       __func__);
 947 +               return ret;
 948 +       }
 949 +
 950 +       ret = clk_prepare_enable(imx219->xclk);
 951 +       if (ret) {
 952 +               dev_err(&client->dev, "%s: failed to enable clock\n",
 953 +                       __func__);
 954 +               goto reg_off;
 955 +       }
 956 +
 957 +       gpiod_set_value_cansleep(imx219->reset_gpio, 1);
 958 +       usleep_range(IMX219_XCLR_MIN_DELAY_US,
 959 +                    IMX219_XCLR_MIN_DELAY_US + IMX219_XCLR_DELAY_RANGE_US);
 960 +
 961 +       return 0;
 962 +
 963 +reg_off:
 964 +       regulator_bulk_disable(IMX219_NUM_SUPPLIES, imx219->supplies);
 965 +
 966 +       return ret;
 967 +}
 968 +
 969 +static int imx219_power_off(struct device *dev)
 970 +{
 971 +       struct i2c_client *client = to_i2c_client(dev);
 972 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
 973 +       struct imx219 *imx219 = to_imx219(sd);
 974 +
 975 +       gpiod_set_value_cansleep(imx219->reset_gpio, 0);
 976 +       regulator_bulk_disable(IMX219_NUM_SUPPLIES, imx219->supplies);
 977 +       clk_disable_unprepare(imx219->xclk);
 978 +
 979 +       return 0;
 980 +}
 981 +
 982 +static int __maybe_unused imx219_suspend(struct device *dev)
 983 +{
 984 +       struct i2c_client *client = to_i2c_client(dev);
 985 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
 986 +       struct imx219 *imx219 = to_imx219(sd);
 987 +
 988 +       if (imx219->streaming)
 989 +               imx219_stop_streaming(imx219);
 990 +
 991 +       return 0;
 992 +}
 993 +
 994 +static int __maybe_unused imx219_resume(struct device *dev)
 995 +{
 996 +       struct i2c_client *client = to_i2c_client(dev);
 997 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
 998 +       struct imx219 *imx219 = to_imx219(sd);
 999 +       int ret;
1000 +
1001 +       if (imx219->streaming) {
1002 +               ret = imx219_start_streaming(imx219);
1003 +               if (ret)
1004 +                       goto error;
1005 +       }
1006 +
1007 +       return 0;
1008 +
1009 +error:
1010 +       imx219_stop_streaming(imx219);
1011 +       imx219->streaming = 0;
1012 +
1013 +       return ret;
1014 +}
1015 +
1016 +static int imx219_get_regulators(struct imx219 *imx219)
1017 +{
1018 +       struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
1019 +       unsigned int i;
1020 +
1021 +       for (i = 0; i < IMX219_NUM_SUPPLIES; i++)
1022 +               imx219->supplies[i].supply = imx219_supply_name[i];
1023 +
1024 +       return devm_regulator_bulk_get(&client->dev,
1025 +                                      IMX219_NUM_SUPPLIES,
1026 +                                      imx219->supplies);
1027 +}
1028 +
1029 +/* Verify chip ID */
1030 +static int imx219_identify_module(struct imx219 *imx219)
1031 +{
1032 +       struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
1033 +       int ret;
1034 +       u32 val;
1035 +
1036 +       ret = imx219_read_reg(imx219, IMX219_REG_CHIP_ID,
1037 +                             IMX219_REG_VALUE_16BIT, &val);
1038 +       if (ret) {
1039 +               dev_err(&client->dev, "failed to read chip id %x\n",
1040 +                       IMX219_CHIP_ID);
1041 +               return ret;
1042 +       }
1043 +
1044 +       if (val != IMX219_CHIP_ID) {
1045 +               dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
1046 +                       IMX219_CHIP_ID, val);
1047 +               return -EIO;
1048 +       }
1049 +
1050 +       return 0;
1051 +}
1052 +
1053 +static const struct v4l2_subdev_core_ops imx219_core_ops = {
1054 +       .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
1055 +       .unsubscribe_event = v4l2_event_subdev_unsubscribe,
1056 +};
1057 +
1058 +static const struct v4l2_subdev_video_ops imx219_video_ops = {
1059 +       .s_stream = imx219_set_stream,
1060 +};
1061 +
1062 +static const struct v4l2_subdev_pad_ops imx219_pad_ops = {
1063 +       .enum_mbus_code = imx219_enum_mbus_code,
1064 +       .get_fmt = imx219_get_pad_format,
1065 +       .set_fmt = imx219_set_pad_format,
1066 +       .enum_frame_size = imx219_enum_frame_size,
1067 +};
1068 +
1069 +static const struct v4l2_subdev_ops imx219_subdev_ops = {
1070 +       .core = &imx219_core_ops,
1071 +       .video = &imx219_video_ops,
1072 +       .pad = &imx219_pad_ops,
1073 +};
1074 +
1075 +static const struct v4l2_subdev_internal_ops imx219_internal_ops = {
1076 +       .open = imx219_open,
1077 +};
1078 +
1079 +/* Initialize control handlers */
1080 +static int imx219_init_controls(struct imx219 *imx219)
1081 +{
1082 +       struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
1083 +       struct v4l2_ctrl_handler *ctrl_hdlr;
1084 +       unsigned int height = imx219->mode->height;
1085 +       int exposure_max, exposure_def, hblank;
1086 +       int i, ret;
1087 +
1088 +       ctrl_hdlr = &imx219->ctrl_handler;
1089 +       ret = v4l2_ctrl_handler_init(ctrl_hdlr, 9);
1090 +       if (ret)
1091 +               return ret;
1092 +
1093 +       mutex_init(&imx219->mutex);
1094 +       ctrl_hdlr->lock = &imx219->mutex;
1095 +
1096 +       /* By default, PIXEL_RATE is read only */
1097 +       imx219->pixel_rate = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
1098 +                                              V4L2_CID_PIXEL_RATE,
1099 +                                              IMX219_PIXEL_RATE,
1100 +                                              IMX219_PIXEL_RATE, 1,
1101 +                                              IMX219_PIXEL_RATE);
1102 +
1103 +       /* Initial vblank/hblank/exposure parameters based on current mode */
1104 +       imx219->vblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
1105 +                                          V4L2_CID_VBLANK, IMX219_VBLANK_MIN,
1106 +                                          IMX219_VTS_MAX - height, 1,
1107 +                                          imx219->mode->vts_def - height);
1108 +       hblank = IMX219_PPL_DEFAULT - imx219->mode->width;
1109 +       imx219->hblank = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
1110 +                                          V4L2_CID_HBLANK, hblank, hblank,
1111 +                                          1, hblank);
1112 +       if (imx219->hblank)
1113 +               imx219->hblank->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1114 +       exposure_max = imx219->mode->vts_def - 4;
1115 +       exposure_def = (exposure_max < IMX219_EXPOSURE_DEFAULT) ?
1116 +               exposure_max : IMX219_EXPOSURE_DEFAULT;
1117 +       imx219->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
1118 +                                            V4L2_CID_EXPOSURE,
1119 +                                            IMX219_EXPOSURE_MIN, exposure_max,
1120 +                                            IMX219_EXPOSURE_STEP,
1121 +                                            exposure_def);
1122 +
1123 +       v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
1124 +                         IMX219_ANA_GAIN_MIN, IMX219_ANA_GAIN_MAX,
1125 +                         IMX219_ANA_GAIN_STEP, IMX219_ANA_GAIN_DEFAULT);
1126 +
1127 +       v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
1128 +                         IMX219_DGTL_GAIN_MIN, IMX219_DGTL_GAIN_MAX,
1129 +                         IMX219_DGTL_GAIN_STEP, IMX219_DGTL_GAIN_DEFAULT);
1130 +
1131 +       imx219->hflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
1132 +                                         V4L2_CID_HFLIP, 0, 1, 1, 0);
1133 +       if (imx219->hflip)
1134 +               imx219->hflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
1135 +
1136 +       imx219->vflip = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
1137 +                                         V4L2_CID_VFLIP, 0, 1, 1, 0);
1138 +       if (imx219->vflip)
1139 +               imx219->vflip->flags |= V4L2_CTRL_FLAG_MODIFY_LAYOUT;
1140 +
1141 +       v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &imx219_ctrl_ops,
1142 +                                    V4L2_CID_TEST_PATTERN,
1143 +                                    ARRAY_SIZE(imx219_test_pattern_menu) - 1,
1144 +                                    0, 0, imx219_test_pattern_menu);
1145 +       for (i = 0; i < 4; i++) {
1146 +               /*
1147 +                * The assumption is that
1148 +                * V4L2_CID_TEST_PATTERN_GREENR == V4L2_CID_TEST_PATTERN_RED + 1
1149 +                * V4L2_CID_TEST_PATTERN_BLUE   == V4L2_CID_TEST_PATTERN_RED + 2
1150 +                * V4L2_CID_TEST_PATTERN_GREENB == V4L2_CID_TEST_PATTERN_RED + 3
1151 +                */
1152 +               v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
1153 +                                 V4L2_CID_TEST_PATTERN_RED + i,
1154 +                                 IMX219_TESTP_COLOUR_MIN,
1155 +                                 IMX219_TESTP_COLOUR_MAX,
1156 +                                 IMX219_TESTP_COLOUR_STEP,
1157 +                                 IMX219_TESTP_COLOUR_MAX);
1158 +               /* The "Solid color" pattern is white by default */
1159 +       }
1160 +
1161 +       if (ctrl_hdlr->error) {
1162 +               ret = ctrl_hdlr->error;
1163 +               dev_err(&client->dev, "%s control init failed (%d)\n",
1164 +                       __func__, ret);
1165 +               goto error;
1166 +       }
1167 +
1168 +       imx219->sd.ctrl_handler = ctrl_hdlr;
1169 +
1170 +       return 0;
1171 +
1172 +error:
1173 +       v4l2_ctrl_handler_free(ctrl_hdlr);
1174 +       mutex_destroy(&imx219->mutex);
1175 +
1176 +       return ret;
1177 +}
1178 +
1179 +static void imx219_free_controls(struct imx219 *imx219)
1180 +{
1181 +       v4l2_ctrl_handler_free(imx219->sd.ctrl_handler);
1182 +       mutex_destroy(&imx219->mutex);
1183 +}
1184 +
1185 +static int imx219_check_hwcfg(struct device *dev)
1186 +{
1187 +       struct fwnode_handle *endpoint;
1188 +       struct v4l2_fwnode_endpoint ep_cfg = {
1189 +               .bus_type = V4L2_MBUS_CSI2_DPHY
1190 +       };
1191 +       int ret = -EINVAL;
1192 +
1193 +       endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(dev), NULL);
1194 +       if (!endpoint) {
1195 +               dev_err(dev, "endpoint node not found\n");
1196 +               return -EINVAL;
1197 +       }
1198 +
1199 +       if (v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep_cfg)) {
1200 +               dev_err(dev, "could not parse endpoint\n");
1201 +               goto error_out;
1202 +       }
1203 +
1204 +       /* Check the number of MIPI CSI2 data lanes */
1205 +       if (ep_cfg.bus.mipi_csi2.num_data_lanes != 2) {
1206 +               dev_err(dev, "only 2 data lanes are currently supported\n");
1207 +               goto error_out;
1208 +       }
1209 +
1210 +       /* Check the link frequency set in device tree */
1211 +       if (!ep_cfg.nr_of_link_frequencies) {
1212 +               dev_err(dev, "link-frequency property not found in DT\n");
1213 +               goto error_out;
1214 +       }
1215 +
1216 +       if (ep_cfg.nr_of_link_frequencies != 1 ||
1217 +           ep_cfg.link_frequencies[0] != IMX219_DEFAULT_LINK_FREQ) {
1218 +               dev_err(dev, "Link frequency not supported: %lld\n",
1219 +                       ep_cfg.link_frequencies[0]);
1220 +               goto error_out;
1221 +       }
1222 +
1223 +       ret = 0;
1224 +
1225 +error_out:
1226 +       v4l2_fwnode_endpoint_free(&ep_cfg);
1227 +       fwnode_handle_put(endpoint);
1228 +
1229 +       return ret;
1230 +}
1231 +
1232 +static int imx219_probe(struct i2c_client *client)
1233 +{
1234 +       struct device *dev = &client->dev;
1235 +       struct imx219 *imx219;
1236 +       int ret;
1237 +
1238 +       imx219 = devm_kzalloc(&client->dev, sizeof(*imx219), GFP_KERNEL);
1239 +       if (!imx219)
1240 +               return -ENOMEM;
1241 +
1242 +       v4l2_i2c_subdev_init(&imx219->sd, client, &imx219_subdev_ops);
1243 +
1244 +       /* Check the hardware configuration in device tree */
1245 +       if (imx219_check_hwcfg(dev))
1246 +               return -EINVAL;
1247 +
1248 +       /* Get system clock (xclk) */
1249 +       imx219->xclk = devm_clk_get(dev, NULL);
1250 +       if (IS_ERR(imx219->xclk)) {
1251 +               dev_err(dev, "failed to get xclk\n");
1252 +               return PTR_ERR(imx219->xclk);
1253 +       }
1254 +
1255 +       imx219->xclk_freq = clk_get_rate(imx219->xclk);
1256 +       if (imx219->xclk_freq != IMX219_XCLK_FREQ) {
1257 +               dev_err(dev, "xclk frequency not supported: %d Hz\n",
1258 +                       imx219->xclk_freq);
1259 +               return -EINVAL;
1260 +       }
1261 +
1262 +       ret = imx219_get_regulators(imx219);
1263 +       if (ret) {
1264 +               dev_err(dev, "failed to get regulators\n");
1265 +               return ret;
1266 +       }
1267 +
1268 +       /* Request optional enable pin */
1269 +       imx219->reset_gpio = devm_gpiod_get_optional(dev, "reset",
1270 +                                                    GPIOD_OUT_HIGH);
1271 +
1272 +       /*
1273 +        * The sensor must be powered for imx219_identify_module()
1274 +        * to be able to read the CHIP_ID register
1275 +        */
1276 +       ret = imx219_power_on(dev);
1277 +       if (ret)
1278 +               return ret;
1279 +
1280 +       ret = imx219_identify_module(imx219);
1281 +       if (ret)
1282 +               goto error_power_off;
1283 +
1284 +       /* Set default mode to max resolution */
1285 +       imx219->mode = &supported_modes[0];
1286 +
1287 +       ret = imx219_init_controls(imx219);
1288 +       if (ret)
1289 +               goto error_power_off;
1290 +
1291 +       /* Initialize subdev */
1292 +       imx219->sd.internal_ops = &imx219_internal_ops;
1293 +       imx219->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1294 +       imx219->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1295 +
1296 +       /* Initialize source pad */
1297 +       imx219->pad.flags = MEDIA_PAD_FL_SOURCE;
1298 +
1299 +       ret = media_entity_pads_init(&imx219->sd.entity, 1, &imx219->pad);
1300 +       if (ret) {
1301 +               dev_err(dev, "failed to init entity pads: %d\n", ret);
1302 +               goto error_handler_free;
1303 +       }
1304 +
1305 +       ret = v4l2_async_register_subdev_sensor_common(&imx219->sd);
1306 +       if (ret < 0) {
1307 +               dev_err(dev, "failed to register sensor sub-device: %d\n", ret);
1308 +               goto error_media_entity;
1309 +       }
1310 +
1311 +       /* Enable runtime PM and turn off the device */
1312 +       pm_runtime_set_active(dev);
1313 +       pm_runtime_enable(dev);
1314 +       pm_runtime_idle(dev);
1315 +
1316 +       return 0;
1317 +
1318 +error_media_entity:
1319 +       media_entity_cleanup(&imx219->sd.entity);
1320 +
1321 +error_handler_free:
1322 +       imx219_free_controls(imx219);
1323 +
1324 +error_power_off:
1325 +       imx219_power_off(dev);
1326 +
1327 +       return ret;
1328 +}
1329 +
1330 +static int imx219_remove(struct i2c_client *client)
1331 +{
1332 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
1333 +       struct imx219 *imx219 = to_imx219(sd);
1334 +
1335 +       v4l2_async_unregister_subdev(sd);
1336 +       media_entity_cleanup(&sd->entity);
1337 +       imx219_free_controls(imx219);
1338 +
1339 +       pm_runtime_disable(&client->dev);
1340 +       if (!pm_runtime_status_suspended(&client->dev))
1341 +               imx219_power_off(&client->dev);
1342 +       pm_runtime_set_suspended(&client->dev);
1343 +
1344 +       return 0;
1345 +}
1346 +
1347 +static const struct of_device_id imx219_dt_ids[] = {
1348 +       { .compatible = "sony,imx219" },
1349 +       { /* sentinel */ }
1350 +};
1351 +MODULE_DEVICE_TABLE(of, imx219_dt_ids);
1352 +
1353 +static const struct dev_pm_ops imx219_pm_ops = {
1354 +       SET_SYSTEM_SLEEP_PM_OPS(imx219_suspend, imx219_resume)
1355 +       SET_RUNTIME_PM_OPS(imx219_power_off, imx219_power_on, NULL)
1356 +};
1357 +
1358 +static struct i2c_driver imx219_i2c_driver = {
1359 +       .driver = {
1360 +               .name = "imx219",
1361 +               .of_match_table = imx219_dt_ids,
1362 +               .pm = &imx219_pm_ops,
1363 +       },
1364 +       .probe_new = imx219_probe,
1365 +       .remove = imx219_remove,
1366 +};
1367 +
1368 +module_i2c_driver(imx219_i2c_driver);
1369 +
1370 +MODULE_AUTHOR("Dave Stevenson <dave.stevenson@raspberrypi.com");
1371 +MODULE_DESCRIPTION("Sony IMX219 sensor driver");
1372 +MODULE_LICENSE("GPL v2");