target/linux/brcm2708/patches-4.19/950-0680-media-i2c-Add-driver-for-Sony-IMX219-sensor.patch

   1 From 2186344c6d83ccd169e16c048c8b43aff95545e2 Mon Sep 17 00:00:00 2001
   2 From: Dave Stevenson <dave.stevenson@raspberrypi.org>
   3 Date: Wed, 28 Aug 2019 13:34:49 +0100
   4 Subject: [PATCH] media: i2c: Add driver for Sony IMX219 sensor
   5
   6 Adds a driver for the 8MPix Sony IMX219 CSI2 sensor.
   7 Whilst the sensor supports 2 or 4 CSI2 data lanes, this driver
   8 currently only supports 2 lanes.
   9 8MPix @ 15fps, 1080P @ 30fps (cropped FOV), and 1640x1232 (2x2 binned)
  10 @ 30fps are currently supported.
  11
  12 Signed-off-by: Dave Stevenson <dave.stevenson@raspberrypi.org>
  13 Tested-by: Kieran Bingham <kieran.bingham@ideasonboard.com>
  14 ---
  15  drivers/media/i2c/Kconfig  |   11 +
  16  drivers/media/i2c/Makefile |    1 +
  17  drivers/media/i2c/imx219.c | 1093 ++++++++++++++++++++++++++++++++++++
  18  3 files changed, 1105 insertions(+)
  19  create mode 100644 drivers/media/i2c/imx219.c
  20
  21 --- a/drivers/media/i2c/Kconfig
  22 +++ b/drivers/media/i2c/Kconfig
  23 @@ -597,6 +597,17 @@ config VIDEO_APTINA_PLL
  24  config VIDEO_SMIAPP_PLL
  25         tristate
  26
  27 +config VIDEO_IMX219
  28 +       tristate "Sony IMX219 sensor support"
  29 +       depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
  30 +       depends on MEDIA_CAMERA_SUPPORT
  31 +       help
  32 +         This is a Video4Linux2 sensor driver for the Sony
  33 +         IMX219 camera.
  34 +
  35 +         To compile this driver as a module, choose M here: the
  36 +         module will be called imx219.
  37 +
  38  config VIDEO_IMX258
  39         tristate "Sony IMX258 sensor support"
  40         depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
  41 --- a/drivers/media/i2c/Makefile
  42 +++ b/drivers/media/i2c/Makefile
  43 @@ -106,6 +106,7 @@ obj-$(CONFIG_VIDEO_I2C)             += video-i2c.o
  44  obj-$(CONFIG_VIDEO_ML86V7667)  += ml86v7667.o
  45  obj-$(CONFIG_VIDEO_OV2659)     += ov2659.o
  46  obj-$(CONFIG_VIDEO_TC358743)   += tc358743.o
  47 +obj-$(CONFIG_VIDEO_IMX219)     += imx219.o
  48  obj-$(CONFIG_VIDEO_IMX258)     += imx258.o
  49  obj-$(CONFIG_VIDEO_IMX274)     += imx274.o
  50
  51 --- /dev/null
  52 +++ b/drivers/media/i2c/imx219.c
  53 @@ -0,0 +1,1093 @@
  54 +// SPDX-License-Identifier: GPL-2.0
  55 +/*
  56 + * A V4L2 driver for Sony IMX219 cameras.
  57 + * Copyright (C) 2019, Raspberry Pi (Trading) Ltd
  58 + *
  59 + * Based on Sony imx258 camera driver
  60 + * Copyright (C) 2018 Intel Corporation
  61 + *
  62 + * DT / fwnode changes, and regulator / GPIO control taken from ov5640.c
  63 + * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved.
  64 + * Copyright (C) 2014-2017 Mentor Graphics Inc.
  65 + *
  66 + */
  67 +
  68 +#include <linux/clk.h>
  69 +#include <linux/clk-provider.h>
  70 +#include <linux/clkdev.h>
  71 +#include <linux/delay.h>
  72 +#include <linux/gpio/consumer.h>
  73 +#include <linux/i2c.h>
  74 +#include <linux/module.h>
  75 +#include <linux/pm_runtime.h>
  76 +#include <linux/regulator/consumer.h>
  77 +#include <media/v4l2-ctrls.h>
  78 +#include <media/v4l2-device.h>
  79 +#include <media/v4l2-fwnode.h>
  80 +#include <media/v4l2-mediabus.h>
  81 +#include <asm/unaligned.h>
  82 +
  83 +#define IMX219_REG_VALUE_08BIT         1
  84 +#define IMX219_REG_VALUE_16BIT         2
  85 +
  86 +#define IMX219_REG_MODE_SELECT         0x0100
  87 +#define IMX219_MODE_STANDBY            0x00
  88 +#define IMX219_MODE_STREAMING          0x01
  89 +
  90 +/* Chip ID */
  91 +#define IMX219_REG_CHIP_ID             0x0000
  92 +#define IMX219_CHIP_ID                 0x0219
  93 +
  94 +/* V_TIMING internal */
  95 +#define IMX219_REG_VTS                 0x0160
  96 +#define IMX219_VTS_15FPS               0x0dc6
  97 +#define IMX219_VTS_30FPS_1080P         0x06e3
  98 +#define IMX219_VTS_30FPS_BINNED                0x06e3
  99 +#define IMX219_VTS_MAX                 0xffff
 100 +
 101 +/*Frame Length Line*/
 102 +#define IMX219_FLL_MIN                 0x08a6
 103 +#define IMX219_FLL_MAX                 0xffff
 104 +#define IMX219_FLL_STEP                        1
 105 +#define IMX219_FLL_DEFAULT             0x0c98
 106 +
 107 +/* HBLANK control - read only */
 108 +#define IMX219_PPL_DEFAULT             5352
 109 +
 110 +/* Exposure control */
 111 +#define IMX219_REG_EXPOSURE            0x015a
 112 +#define IMX219_EXPOSURE_MIN            4
 113 +#define IMX219_EXPOSURE_STEP           1
 114 +#define IMX219_EXPOSURE_DEFAULT                0x640
 115 +#define IMX219_EXPOSURE_MAX            65535
 116 +
 117 +/* Analog gain control */
 118 +#define IMX219_REG_ANALOG_GAIN         0x0157
 119 +#define IMX219_ANA_GAIN_MIN            0
 120 +#define IMX219_ANA_GAIN_MAX            232
 121 +#define IMX219_ANA_GAIN_STEP           1
 122 +#define IMX219_ANA_GAIN_DEFAULT                0x0
 123 +
 124 +/* Digital gain control */
 125 +#define IMX219_REG_DIGITAL_GAIN                0x0158
 126 +#define IMX219_DGTL_GAIN_MIN           0x0100
 127 +#define IMX219_DGTL_GAIN_MAX           0x0fff
 128 +#define IMX219_DGTL_GAIN_DEFAULT       0x0100
 129 +#define IMX219_DGTL_GAIN_STEP          1
 130 +
 131 +/* Test Pattern Control */
 132 +#define IMX219_REG_TEST_PATTERN                0x0600
 133 +#define IMX219_TEST_PATTERN_DISABLE    0
 134 +#define IMX219_TEST_PATTERN_SOLID_COLOR        1
 135 +#define IMX219_TEST_PATTERN_COLOR_BARS 2
 136 +#define IMX219_TEST_PATTERN_GREY_COLOR 3
 137 +#define IMX219_TEST_PATTERN_PN9                4
 138 +
 139 +struct imx219_reg {
 140 +       u16 address;
 141 +       u8 val;
 142 +};
 143 +
 144 +struct imx219_reg_list {
 145 +       u32 num_of_regs;
 146 +       const struct imx219_reg *regs;
 147 +};
 148 +
 149 +/* Mode : resolution and related config&values */
 150 +struct imx219_mode {
 151 +       /* Frame width */
 152 +       u32 width;
 153 +       /* Frame height */
 154 +       u32 height;
 155 +
 156 +       /* V-timing */
 157 +       u32 vts_def;
 158 +
 159 +       /* Default register values */
 160 +       struct imx219_reg_list reg_list;
 161 +};
 162 +
 163 +/*
 164 + * Register sets lifted off the i2C interface from the Raspberry Pi firmware
 165 + * driver.
 166 + * 3280x2464 = mode 2, 1920x1080 = mode 1, and 1640x1232 = mode 4.
 167 + */
 168 +static const struct imx219_reg mode_3280x2464_regs[] = {
 169 +       {0x0100, 0x00},
 170 +       {0x30eb, 0x0c},
 171 +       {0x30eb, 0x05},
 172 +       {0x300a, 0xff},
 173 +       {0x300b, 0xff},
 174 +       {0x30eb, 0x05},
 175 +       {0x30eb, 0x09},
 176 +       {0x0114, 0x01},
 177 +       {0x0128, 0x00},
 178 +       {0x012a, 0x18},
 179 +       {0x012b, 0x00},
 180 +       {0x0164, 0x00},
 181 +       {0x0165, 0x00},
 182 +       {0x0166, 0x0c},
 183 +       {0x0167, 0xcf},
 184 +       {0x0168, 0x00},
 185 +       {0x0169, 0x00},
 186 +       {0x016a, 0x09},
 187 +       {0x016b, 0x9f},
 188 +       {0x016c, 0x0c},
 189 +       {0x016d, 0xd0},
 190 +       {0x016e, 0x09},
 191 +       {0x016f, 0xa0},
 192 +       {0x0170, 0x01},
 193 +       {0x0171, 0x01},
 194 +       {0x0174, 0x00},
 195 +       {0x0175, 0x00},
 196 +       {0x018c, 0x0a},
 197 +       {0x018d, 0x0a},
 198 +       {0x0301, 0x05},
 199 +       {0x0303, 0x01},
 200 +       {0x0304, 0x03},
 201 +       {0x0305, 0x03},
 202 +       {0x0306, 0x00},
 203 +       {0x0307, 0x39},
 204 +       {0x0309, 0x0a},
 205 +       {0x030b, 0x01},
 206 +       {0x030c, 0x00},
 207 +       {0x030d, 0x72},
 208 +       {0x0624, 0x0c},
 209 +       {0x0625, 0xd0},
 210 +       {0x0626, 0x09},
 211 +       {0x0627, 0xa0},
 212 +       {0x455e, 0x00},
 213 +       {0x471e, 0x4b},
 214 +       {0x4767, 0x0f},
 215 +       {0x4750, 0x14},
 216 +       {0x4540, 0x00},
 217 +       {0x47b4, 0x14},
 218 +       {0x4713, 0x30},
 219 +       {0x478b, 0x10},
 220 +       {0x478f, 0x10},
 221 +       {0x4793, 0x10},
 222 +       {0x4797, 0x0e},
 223 +       {0x479b, 0x0e},
 224 +
 225 +       {0x0172, 0x03},
 226 +       {0x0162, 0x0d},
 227 +       {0x0163, 0x78},
 228 +};
 229 +
 230 +static const struct imx219_reg mode_1920_1080_regs[] = {
 231 +       {0x0100, 0x00},
 232 +       {0x30eb, 0x05},
 233 +       {0x30eb, 0x0c},
 234 +       {0x300a, 0xff},
 235 +       {0x300b, 0xff},
 236 +       {0x30eb, 0x05},
 237 +       {0x30eb, 0x09},
 238 +       {0x0114, 0x01},
 239 +       {0x0128, 0x00},
 240 +       {0x012a, 0x18},
 241 +       {0x012b, 0x00},
 242 +       {0x0162, 0x0d},
 243 +       {0x0163, 0x78},
 244 +       {0x0164, 0x02},
 245 +       {0x0165, 0xa8},
 246 +       {0x0166, 0x0a},
 247 +       {0x0167, 0x27},
 248 +       {0x0168, 0x02},
 249 +       {0x0169, 0xb4},
 250 +       {0x016a, 0x06},
 251 +       {0x016b, 0xeb},
 252 +       {0x016c, 0x07},
 253 +       {0x016d, 0x80},
 254 +       {0x016e, 0x04},
 255 +       {0x016f, 0x38},
 256 +       {0x0170, 0x01},
 257 +       {0x0171, 0x01},
 258 +       {0x0174, 0x00},
 259 +       {0x0175, 0x00},
 260 +       {0x018c, 0x0a},
 261 +       {0x018d, 0x0a},
 262 +       {0x0301, 0x05},
 263 +       {0x0303, 0x01},
 264 +       {0x0304, 0x03},
 265 +       {0x0305, 0x03},
 266 +       {0x0306, 0x00},
 267 +       {0x0307, 0x39},
 268 +       {0x0309, 0x0a},
 269 +       {0x030b, 0x01},
 270 +       {0x030c, 0x00},
 271 +       {0x030d, 0x72},
 272 +       {0x455e, 0x00},
 273 +       {0x471e, 0x4b},
 274 +       {0x4767, 0x0f},
 275 +       {0x4750, 0x14},
 276 +       {0x4540, 0x00},
 277 +       {0x47b4, 0x14},
 278 +       {0x4713, 0x30},
 279 +       {0x478b, 0x10},
 280 +       {0x478f, 0x10},
 281 +       {0x4793, 0x10},
 282 +       {0x4797, 0x0e},
 283 +       {0x479b, 0x0e},
 284 +
 285 +       {0x0172, 0x03},
 286 +       {0x0162, 0x0d},
 287 +       {0x0163, 0x78},
 288 +};
 289 +
 290 +static const struct imx219_reg mode_1640_1232_regs[] = {
 291 +       {0x30eb, 0x0c},
 292 +       {0x30eb, 0x05},
 293 +       {0x300a, 0xff},
 294 +       {0x300b, 0xff},
 295 +       {0x30eb, 0x05},
 296 +       {0x30eb, 0x09},
 297 +       {0x0114, 0x01},
 298 +       {0x0128, 0x00},
 299 +       {0x012a, 0x18},
 300 +       {0x012b, 0x00},
 301 +       {0x0164, 0x00},
 302 +       {0x0165, 0x00},
 303 +       {0x0166, 0x0c},
 304 +       {0x0167, 0xcf},
 305 +       {0x0168, 0x00},
 306 +       {0x0169, 0x00},
 307 +       {0x016a, 0x09},
 308 +       {0x016b, 0x9f},
 309 +       {0x016c, 0x06},
 310 +       {0x016d, 0x68},
 311 +       {0x016e, 0x04},
 312 +       {0x016f, 0xd0},
 313 +       {0x0170, 0x01},
 314 +       {0x0171, 0x01},
 315 +       {0x0174, 0x01},
 316 +       {0x0175, 0x01},
 317 +       {0x018c, 0x0a},
 318 +       {0x018d, 0x0a},
 319 +       {0x0301, 0x05},
 320 +       {0x0303, 0x01},
 321 +       {0x0304, 0x03},
 322 +       {0x0305, 0x03},
 323 +       {0x0306, 0x00},
 324 +       {0x0307, 0x39},
 325 +       {0x0309, 0x0a},
 326 +       {0x030b, 0x01},
 327 +       {0x030c, 0x00},
 328 +       {0x030d, 0x72},
 329 +       {0x455e, 0x00},
 330 +       {0x471e, 0x4b},
 331 +       {0x4767, 0x0f},
 332 +       {0x4750, 0x14},
 333 +       {0x4540, 0x00},
 334 +       {0x47b4, 0x14},
 335 +       {0x4713, 0x30},
 336 +       {0x478b, 0x10},
 337 +       {0x478f, 0x10},
 338 +       {0x4793, 0x10},
 339 +       {0x4797, 0x0e},
 340 +       {0x479b, 0x0e},
 341 +
 342 +       {0x0172, 0x03},
 343 +       {0x0162, 0x0d},
 344 +       {0x0163, 0x78},
 345 +};
 346 +
 347 +static const char * const imx219_test_pattern_menu[] = {
 348 +       "Disabled",
 349 +       "Color Bars",
 350 +       "Solid Color",
 351 +       "Grey Color Bars",
 352 +       "PN9"
 353 +};
 354 +
 355 +static const int imx219_test_pattern_val[] = {
 356 +       IMX219_TEST_PATTERN_DISABLE,
 357 +       IMX219_TEST_PATTERN_COLOR_BARS,
 358 +       IMX219_TEST_PATTERN_SOLID_COLOR,
 359 +       IMX219_TEST_PATTERN_GREY_COLOR,
 360 +       IMX219_TEST_PATTERN_PN9,
 361 +};
 362 +
 363 +/* regulator supplies */
 364 +static const char * const imx219_supply_name[] = {
 365 +       /* Supplies can be enabled in any order */
 366 +       "VANA",  /* Analog (2.8V) supply */
 367 +       "VDIG",  /* Digital Core (1.8V) supply */
 368 +       "VDDL",  /* IF (1.2V) supply */
 369 +};
 370 +
 371 +#define IMX219_NUM_SUPPLIES ARRAY_SIZE(imx219_supply_name)
 372 +
 373 +#define IMX219_XCLR_DELAY_MS 10        /* Initialisation delay after XCLR low->high */
 374 +
 375 +/* Mode configs */
 376 +static const struct imx219_mode supported_modes[] = {
 377 +       {
 378 +               /* 8MPix 15fps mode */
 379 +               .width = 3280,
 380 +               .height = 2464,
 381 +               .vts_def = IMX219_VTS_15FPS,
 382 +               .reg_list = {
 383 +                       .num_of_regs = ARRAY_SIZE(mode_3280x2464_regs),
 384 +                       .regs = mode_3280x2464_regs,
 385 +               },
 386 +       },
 387 +       {
 388 +               /* 1080P 30fps cropped */
 389 +               .width = 1920,
 390 +               .height = 1080,
 391 +               .vts_def = IMX219_VTS_30FPS_1080P,
 392 +               .reg_list = {
 393 +                       .num_of_regs = ARRAY_SIZE(mode_1920_1080_regs),
 394 +                       .regs = mode_1920_1080_regs,
 395 +               },
 396 +       },
 397 +       {
 398 +               /* 2x2 binned 30fps mode */
 399 +               .width = 1640,
 400 +               .height = 1232,
 401 +               .vts_def = IMX219_VTS_30FPS_BINNED,
 402 +               .reg_list = {
 403 +                       .num_of_regs = ARRAY_SIZE(mode_1640_1232_regs),
 404 +                       .regs = mode_1640_1232_regs,
 405 +               },
 406 +       },
 407 +};
 408 +
 409 +struct imx219 {
 410 +       struct v4l2_subdev sd;
 411 +       struct media_pad pad;
 412 +
 413 +       struct v4l2_fwnode_endpoint ep; /* the parsed DT endpoint info */
 414 +       struct clk *xclk; /* system clock to IMX219 */
 415 +       u32 xclk_freq;
 416 +
 417 +       struct gpio_desc *xclr_gpio;
 418 +       struct regulator_bulk_data supplies[IMX219_NUM_SUPPLIES];
 419 +
 420 +       struct v4l2_ctrl_handler ctrl_handler;
 421 +       /* V4L2 Controls */
 422 +       struct v4l2_ctrl *pixel_rate;
 423 +       struct v4l2_ctrl *exposure;
 424 +
 425 +       /* Current mode */
 426 +       const struct imx219_mode *mode;
 427 +
 428 +       /*
 429 +        * Mutex for serialized access:
 430 +        * Protect sensor module set pad format and start/stop streaming safely.
 431 +        */
 432 +       struct mutex mutex;
 433 +
 434 +       int power_count;
 435 +       /* Streaming on/off */
 436 +       bool streaming;
 437 +};
 438 +
 439 +static inline struct imx219 *to_imx219(struct v4l2_subdev *_sd)
 440 +{
 441 +       return container_of(_sd, struct imx219, sd);
 442 +}
 443 +
 444 +/* Read registers up to 2 at a time */
 445 +static int imx219_read_reg(struct imx219 *imx219, u16 reg, u32 len, u32 *val)
 446 +{
 447 +       struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 448 +       struct i2c_msg msgs[2];
 449 +       u8 addr_buf[2] = { reg >> 8, reg & 0xff };
 450 +       u8 data_buf[4] = { 0, };
 451 +       int ret;
 452 +
 453 +       if (len > 4)
 454 +               return -EINVAL;
 455 +
 456 +       /* Write register address */
 457 +       msgs[0].addr = client->addr;
 458 +       msgs[0].flags = 0;
 459 +       msgs[0].len = ARRAY_SIZE(addr_buf);
 460 +       msgs[0].buf = addr_buf;
 461 +
 462 +       /* Read data from register */
 463 +       msgs[1].addr = client->addr;
 464 +       msgs[1].flags = I2C_M_RD;
 465 +       msgs[1].len = len;
 466 +       msgs[1].buf = &data_buf[4 - len];
 467 +
 468 +       ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
 469 +       if (ret != ARRAY_SIZE(msgs))
 470 +               return -EIO;
 471 +
 472 +       *val = get_unaligned_be32(data_buf);
 473 +
 474 +       return 0;
 475 +}
 476 +
 477 +/* Write registers up to 2 at a time */
 478 +static int imx219_write_reg(struct imx219 *imx219, u16 reg, u32 len, u32 val)
 479 +{
 480 +       struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 481 +       u8 buf[6];
 482 +
 483 +       if (len > 4)
 484 +               return -EINVAL;
 485 +
 486 +       put_unaligned_be16(reg, buf);
 487 +       put_unaligned_be32(val << (8 * (4 - len)), buf + 2);
 488 +       if (i2c_master_send(client, buf, len + 2) != len + 2)
 489 +               return -EIO;
 490 +
 491 +       return 0;
 492 +}
 493 +
 494 +/* Write a list of registers */
 495 +static int imx219_write_regs(struct imx219 *imx219,
 496 +                            const struct imx219_reg *regs, u32 len)
 497 +{
 498 +       struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 499 +       unsigned int i;
 500 +       int ret;
 501 +
 502 +       for (i = 0; i < len; i++) {
 503 +               ret = imx219_write_reg(imx219, regs[i].address, 1, regs[i].val);
 504 +               if (ret) {
 505 +                       dev_err_ratelimited(&client->dev,
 506 +                                           "Failed to write reg 0x%4.4x. error = %d\n",
 507 +                                           regs[i].address, ret);
 508 +
 509 +                       return ret;
 510 +               }
 511 +       }
 512 +
 513 +       return 0;
 514 +}
 515 +
 516 +/* Power/clock management functions */
 517 +static void imx219_power(struct imx219 *imx219, bool enable)
 518 +{
 519 +       gpiod_set_value_cansleep(imx219->xclr_gpio, enable ? 1 : 0);
 520 +}
 521 +
 522 +static int imx219_set_power_on(struct imx219 *imx219)
 523 +{
 524 +       struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 525 +       int ret;
 526 +
 527 +       ret = clk_prepare_enable(imx219->xclk);
 528 +       if (ret) {
 529 +               dev_err(&client->dev, "%s: failed to enable clock\n",
 530 +                       __func__);
 531 +               return ret;
 532 +       }
 533 +
 534 +       ret = regulator_bulk_enable(IMX219_NUM_SUPPLIES,
 535 +                                   imx219->supplies);
 536 +       if (ret) {
 537 +               dev_err(&client->dev, "%s: failed to enable regulators\n",
 538 +                       __func__);
 539 +               goto xclk_off;
 540 +       }
 541 +
 542 +       imx219_power(imx219, true);
 543 +       msleep(IMX219_XCLR_DELAY_MS);
 544 +
 545 +       return 0;
 546 +xclk_off:
 547 +       clk_disable_unprepare(imx219->xclk);
 548 +       return ret;
 549 +}
 550 +
 551 +static void imx219_set_power_off(struct imx219 *imx219)
 552 +{
 553 +       imx219_power(imx219, false);
 554 +       regulator_bulk_disable(IMX219_NUM_SUPPLIES, imx219->supplies);
 555 +       clk_disable_unprepare(imx219->xclk);
 556 +}
 557 +
 558 +static int imx219_set_power(struct imx219 *imx219, bool on)
 559 +{
 560 +       int ret = 0;
 561 +
 562 +       if (on) {
 563 +               ret = imx219_set_power_on(imx219);
 564 +               if (ret)
 565 +                       return ret;
 566 +       } else {
 567 +               imx219_set_power_off(imx219);
 568 +       }
 569 +
 570 +       return 0;
 571 +}
 572 +
 573 +/* Open sub-device */
 574 +static int imx219_s_power(struct v4l2_subdev *sd, int on)
 575 +{
 576 +       struct imx219 *imx219 = to_imx219(sd);
 577 +       int ret = 0;
 578 +
 579 +       mutex_lock(&imx219->mutex);
 580 +
 581 +       /*
 582 +        * If the power count is modified from 0 to != 0 or from != 0 to 0,
 583 +        * update the power state.
 584 +        */
 585 +       if (imx219->power_count == !on) {
 586 +               ret = imx219_set_power(imx219, !!on);
 587 +               if (ret)
 588 +                       goto out;
 589 +       }
 590 +
 591 +       /* Update the power count. */
 592 +       imx219->power_count += on ? 1 : -1;
 593 +       WARN_ON(imx219->power_count < 0);
 594 +out:
 595 +       mutex_unlock(&imx219->mutex);
 596 +
 597 +       return ret;
 598 +}
 599 +
 600 +static int imx219_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
 601 +{
 602 +       struct v4l2_mbus_framefmt *try_fmt =
 603 +               v4l2_subdev_get_try_format(sd, fh->pad, 0);
 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 = MEDIA_BUS_FMT_SBGGR10_1X10;
 609 +       try_fmt->field = V4L2_FIELD_NONE;
 610 +
 611 +       return 0;
 612 +}
 613 +
 614 +static int imx219_set_ctrl(struct v4l2_ctrl *ctrl)
 615 +{
 616 +       struct imx219 *imx219 =
 617 +               container_of(ctrl->handler, struct imx219, ctrl_handler);
 618 +       struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 619 +       int ret = 0;
 620 +
 621 +       /*
 622 +        * Applying V4L2 control value only happens
 623 +        * when power is up for streaming
 624 +        */
 625 +       if (pm_runtime_get_if_in_use(&client->dev) == 0)
 626 +               return 0;
 627 +
 628 +       switch (ctrl->id) {
 629 +       case V4L2_CID_ANALOGUE_GAIN:
 630 +               ret = imx219_write_reg(imx219, IMX219_REG_ANALOG_GAIN,
 631 +                                      IMX219_REG_VALUE_08BIT, ctrl->val);
 632 +               break;
 633 +       case V4L2_CID_EXPOSURE:
 634 +               ret = imx219_write_reg(imx219, IMX219_REG_EXPOSURE,
 635 +                                      IMX219_REG_VALUE_16BIT, ctrl->val);
 636 +               break;
 637 +       case V4L2_CID_DIGITAL_GAIN:
 638 +               ret = imx219_write_reg(imx219, IMX219_REG_DIGITAL_GAIN,
 639 +                                      IMX219_REG_VALUE_16BIT, ctrl->val);
 640 +               break;
 641 +       case V4L2_CID_TEST_PATTERN:
 642 +               ret = imx219_write_reg(imx219, IMX219_REG_TEST_PATTERN,
 643 +                                      IMX219_REG_VALUE_16BIT,
 644 +                                      imx219_test_pattern_val[ctrl->val]);
 645 +               break;
 646 +       default:
 647 +               dev_info(&client->dev,
 648 +                        "ctrl(id:0x%x,val:0x%x) is not handled\n",
 649 +                        ctrl->id, ctrl->val);
 650 +               ret = -EINVAL;
 651 +               break;
 652 +       }
 653 +
 654 +       pm_runtime_put(&client->dev);
 655 +
 656 +       return ret;
 657 +}
 658 +
 659 +static const struct v4l2_ctrl_ops imx219_ctrl_ops = {
 660 +       .s_ctrl = imx219_set_ctrl,
 661 +};
 662 +
 663 +static int imx219_enum_mbus_code(struct v4l2_subdev *sd,
 664 +                                struct v4l2_subdev_pad_config *cfg,
 665 +                                struct v4l2_subdev_mbus_code_enum *code)
 666 +{
 667 +       /* Only one bayer order(GRBG) is supported */
 668 +       if (code->index > 0)
 669 +               return -EINVAL;
 670 +
 671 +       code->code = MEDIA_BUS_FMT_SBGGR10_1X10;
 672 +
 673 +       return 0;
 674 +}
 675 +
 676 +static int imx219_enum_frame_size(struct v4l2_subdev *sd,
 677 +                                 struct v4l2_subdev_pad_config *cfg,
 678 +                                 struct v4l2_subdev_frame_size_enum *fse)
 679 +{
 680 +       if (fse->index >= ARRAY_SIZE(supported_modes))
 681 +               return -EINVAL;
 682 +
 683 +       if (fse->code != MEDIA_BUS_FMT_SBGGR10_1X10)
 684 +               return -EINVAL;
 685 +
 686 +       fse->min_width = supported_modes[fse->index].width;
 687 +       fse->max_width = fse->min_width;
 688 +       fse->min_height = supported_modes[fse->index].height;
 689 +       fse->max_height = fse->min_height;
 690 +
 691 +       return 0;
 692 +}
 693 +
 694 +static void imx219_update_pad_format(const struct imx219_mode *mode,
 695 +                                    struct v4l2_subdev_format *fmt)
 696 +{
 697 +       fmt->format.width = mode->width;
 698 +       fmt->format.height = mode->height;
 699 +       fmt->format.code = MEDIA_BUS_FMT_SBGGR10_1X10;
 700 +       fmt->format.field = V4L2_FIELD_NONE;
 701 +}
 702 +
 703 +static int __imx219_get_pad_format(struct imx219 *imx219,
 704 +                                  struct v4l2_subdev_pad_config *cfg,
 705 +                                  struct v4l2_subdev_format *fmt)
 706 +{
 707 +       if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
 708 +               fmt->format = *v4l2_subdev_get_try_format(&imx219->sd, cfg,
 709 +                                                         fmt->pad);
 710 +       else
 711 +               imx219_update_pad_format(imx219->mode, fmt);
 712 +
 713 +       return 0;
 714 +}
 715 +
 716 +static int imx219_get_pad_format(struct v4l2_subdev *sd,
 717 +                                struct v4l2_subdev_pad_config *cfg,
 718 +                                struct v4l2_subdev_format *fmt)
 719 +{
 720 +       struct imx219 *imx219 = to_imx219(sd);
 721 +       int ret;
 722 +
 723 +       mutex_lock(&imx219->mutex);
 724 +       ret = __imx219_get_pad_format(imx219, cfg, fmt);
 725 +       mutex_unlock(&imx219->mutex);
 726 +
 727 +       return ret;
 728 +}
 729 +
 730 +static int imx219_set_pad_format(struct v4l2_subdev *sd,
 731 +                                struct v4l2_subdev_pad_config *cfg,
 732 +                                struct v4l2_subdev_format *fmt)
 733 +{
 734 +       struct imx219 *imx219 = to_imx219(sd);
 735 +       const struct imx219_mode *mode;
 736 +       struct v4l2_mbus_framefmt *framefmt;
 737 +
 738 +       mutex_lock(&imx219->mutex);
 739 +
 740 +       /* Only one raw bayer(BGGR) order is supported */
 741 +       fmt->format.code = MEDIA_BUS_FMT_SBGGR10_1X10;
 742 +
 743 +       mode = v4l2_find_nearest_size(supported_modes,
 744 +                                     ARRAY_SIZE(supported_modes),
 745 +                                     width, height,
 746 +                                     fmt->format.width, fmt->format.height);
 747 +       imx219_update_pad_format(mode, fmt);
 748 +       if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
 749 +               framefmt = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
 750 +               *framefmt = fmt->format;
 751 +       } else {
 752 +               imx219->mode = mode;
 753 +       }
 754 +
 755 +       mutex_unlock(&imx219->mutex);
 756 +
 757 +       return 0;
 758 +}
 759 +
 760 +/* Start streaming */
 761 +static int imx219_start_streaming(struct imx219 *imx219)
 762 +{
 763 +       struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 764 +       const struct imx219_reg_list *reg_list;
 765 +       int ret;
 766 +
 767 +       /* Apply default values of current mode */
 768 +       reg_list = &imx219->mode->reg_list;
 769 +       ret = imx219_write_regs(imx219, reg_list->regs, reg_list->num_of_regs);
 770 +       if (ret) {
 771 +               dev_err(&client->dev, "%s failed to set mode\n", __func__);
 772 +               return ret;
 773 +       }
 774 +
 775 +       /*
 776 +        * Set VTS appropriately for frame rate control.
 777 +        * Currently fixed per mode.
 778 +        */
 779 +       ret = imx219_write_reg(imx219, IMX219_REG_VTS,
 780 +                              IMX219_REG_VALUE_16BIT, imx219->mode->vts_def);
 781 +       if (ret)
 782 +               return ret;
 783 +
 784 +       /* Apply customized values from user */
 785 +       ret =  __v4l2_ctrl_handler_setup(imx219->sd.ctrl_handler);
 786 +       if (ret)
 787 +               return ret;
 788 +
 789 +       /* set stream on register */
 790 +       return imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
 791 +                               IMX219_REG_VALUE_08BIT, IMX219_MODE_STREAMING);
 792 +}
 793 +
 794 +/* Stop streaming */
 795 +static int imx219_stop_streaming(struct imx219 *imx219)
 796 +{
 797 +       struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 798 +       int ret;
 799 +
 800 +       /* set stream off register */
 801 +       ret = imx219_write_reg(imx219, IMX219_REG_MODE_SELECT,
 802 +                              IMX219_REG_VALUE_08BIT, IMX219_MODE_STANDBY);
 803 +       if (ret)
 804 +               dev_err(&client->dev, "%s failed to set stream\n", __func__);
 805 +
 806 +       /*
 807 +        * Return success even if it was an error, as there is nothing the
 808 +        * caller can do about it.
 809 +        */
 810 +       return 0;
 811 +}
 812 +
 813 +static int imx219_set_stream(struct v4l2_subdev *sd, int enable)
 814 +{
 815 +       struct imx219 *imx219 = to_imx219(sd);
 816 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
 817 +       int ret = 0;
 818 +
 819 +       mutex_lock(&imx219->mutex);
 820 +       if (imx219->streaming == enable) {
 821 +               mutex_unlock(&imx219->mutex);
 822 +               return 0;
 823 +       }
 824 +
 825 +       if (enable) {
 826 +               ret = pm_runtime_get_sync(&client->dev);
 827 +               if (ret < 0) {
 828 +                       pm_runtime_put_noidle(&client->dev);
 829 +                       goto err_unlock;
 830 +               }
 831 +
 832 +               /*
 833 +                * Apply default & customized values
 834 +                * and then start streaming.
 835 +                */
 836 +               ret = imx219_start_streaming(imx219);
 837 +               if (ret) {
 838 +                       pm_runtime_put(&client->dev);
 839 +                       goto err_unlock;
 840 +               }
 841 +       } else {
 842 +               imx219_stop_streaming(imx219);
 843 +               pm_runtime_put(&client->dev);
 844 +       }
 845 +
 846 +       imx219->streaming = enable;
 847 +       mutex_unlock(&imx219->mutex);
 848 +
 849 +       return ret;
 850 +
 851 +err_unlock:
 852 +       mutex_unlock(&imx219->mutex);
 853 +
 854 +       return ret;
 855 +}
 856 +
 857 +static int __maybe_unused imx219_suspend(struct device *dev)
 858 +{
 859 +       struct i2c_client *client = to_i2c_client(dev);
 860 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
 861 +       struct imx219 *imx219 = to_imx219(sd);
 862 +
 863 +       if (imx219->streaming)
 864 +               imx219_stop_streaming(imx219);
 865 +
 866 +       return 0;
 867 +}
 868 +
 869 +static int __maybe_unused imx219_resume(struct device *dev)
 870 +{
 871 +       struct i2c_client *client = to_i2c_client(dev);
 872 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
 873 +       struct imx219 *imx219 = to_imx219(sd);
 874 +       int ret;
 875 +
 876 +       if (imx219->streaming) {
 877 +               ret = imx219_start_streaming(imx219);
 878 +               if (ret)
 879 +                       goto error;
 880 +       }
 881 +
 882 +       return 0;
 883 +
 884 +error:
 885 +       imx219_stop_streaming(imx219);
 886 +       imx219->streaming = 0;
 887 +       return ret;
 888 +}
 889 +
 890 +static int imx219_get_regulators(struct imx219 *imx219)
 891 +{
 892 +       struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 893 +       int i;
 894 +
 895 +       for (i = 0; i < IMX219_NUM_SUPPLIES; i++)
 896 +               imx219->supplies[i].supply = imx219_supply_name[i];
 897 +
 898 +       return devm_regulator_bulk_get(&client->dev,
 899 +                                      IMX219_NUM_SUPPLIES,
 900 +                                      imx219->supplies);
 901 +}
 902 +
 903 +/* Verify chip ID */
 904 +static int imx219_identify_module(struct imx219 *imx219)
 905 +{
 906 +       struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 907 +       int ret;
 908 +       u32 val;
 909 +
 910 +       ret = imx219_set_power_on(imx219);
 911 +       if (ret)
 912 +               return ret;
 913 +
 914 +       ret = imx219_read_reg(imx219, IMX219_REG_CHIP_ID,
 915 +                             IMX219_REG_VALUE_16BIT, &val);
 916 +       if (ret) {
 917 +               dev_err(&client->dev, "failed to read chip id %x\n",
 918 +                       IMX219_CHIP_ID);
 919 +               goto power_off;
 920 +       }
 921 +
 922 +       if (val != IMX219_CHIP_ID) {
 923 +               dev_err(&client->dev, "chip id mismatch: %x!=%x\n",
 924 +                       IMX219_CHIP_ID, val);
 925 +               ret = -EIO;
 926 +       }
 927 +
 928 +power_off:
 929 +       imx219_set_power_off(imx219);
 930 +       return ret;
 931 +}
 932 +
 933 +static const struct v4l2_subdev_core_ops imx219_core_ops = {
 934 +       .s_power = imx219_s_power,
 935 +};
 936 +
 937 +static const struct v4l2_subdev_video_ops imx219_video_ops = {
 938 +       .s_stream = imx219_set_stream,
 939 +};
 940 +
 941 +static const struct v4l2_subdev_pad_ops imx219_pad_ops = {
 942 +       .enum_mbus_code = imx219_enum_mbus_code,
 943 +       .get_fmt = imx219_get_pad_format,
 944 +       .set_fmt = imx219_set_pad_format,
 945 +       .enum_frame_size = imx219_enum_frame_size,
 946 +};
 947 +
 948 +static const struct v4l2_subdev_ops imx219_subdev_ops = {
 949 +       .core = &imx219_core_ops,
 950 +       .video = &imx219_video_ops,
 951 +       .pad = &imx219_pad_ops,
 952 +};
 953 +
 954 +static const struct v4l2_subdev_internal_ops imx219_internal_ops = {
 955 +       .open = imx219_open,
 956 +};
 957 +
 958 +/* Initialize control handlers */
 959 +static int imx219_init_controls(struct imx219 *imx219)
 960 +{
 961 +       struct i2c_client *client = v4l2_get_subdevdata(&imx219->sd);
 962 +       struct v4l2_ctrl_handler *ctrl_hdlr;
 963 +       int ret;
 964 +
 965 +       ctrl_hdlr = &imx219->ctrl_handler;
 966 +       ret = v4l2_ctrl_handler_init(ctrl_hdlr, 8);
 967 +       if (ret)
 968 +               return ret;
 969 +
 970 +       mutex_init(&imx219->mutex);
 971 +       ctrl_hdlr->lock = &imx219->mutex;
 972 +
 973 +       imx219->exposure = v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops,
 974 +                                            V4L2_CID_EXPOSURE,
 975 +                                            IMX219_EXPOSURE_MIN,
 976 +                                            IMX219_EXPOSURE_MAX,
 977 +                                            IMX219_EXPOSURE_STEP,
 978 +                                            IMX219_EXPOSURE_DEFAULT);
 979 +
 980 +       v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops, V4L2_CID_ANALOGUE_GAIN,
 981 +                         IMX219_ANA_GAIN_MIN, IMX219_ANA_GAIN_MAX,
 982 +                         IMX219_ANA_GAIN_STEP, IMX219_ANA_GAIN_DEFAULT);
 983 +
 984 +       v4l2_ctrl_new_std(ctrl_hdlr, &imx219_ctrl_ops, V4L2_CID_DIGITAL_GAIN,
 985 +                         IMX219_DGTL_GAIN_MIN, IMX219_DGTL_GAIN_MAX,
 986 +                         IMX219_DGTL_GAIN_STEP, IMX219_DGTL_GAIN_DEFAULT);
 987 +
 988 +       v4l2_ctrl_new_std_menu_items(ctrl_hdlr, &imx219_ctrl_ops,
 989 +                                    V4L2_CID_TEST_PATTERN,
 990 +                                    ARRAY_SIZE(imx219_test_pattern_menu) - 1,
 991 +                                    0, 0, imx219_test_pattern_menu);
 992 +
 993 +       if (ctrl_hdlr->error) {
 994 +               ret = ctrl_hdlr->error;
 995 +               dev_err(&client->dev, "%s control init failed (%d)\n",
 996 +                       __func__, ret);
 997 +               goto error;
 998 +       }
 999 +
1000 +       imx219->sd.ctrl_handler = ctrl_hdlr;
1001 +
1002 +       return 0;
1003 +
1004 +error:
1005 +       v4l2_ctrl_handler_free(ctrl_hdlr);
1006 +       mutex_destroy(&imx219->mutex);
1007 +
1008 +       return ret;
1009 +}
1010 +
1011 +static void imx219_free_controls(struct imx219 *imx219)
1012 +{
1013 +       v4l2_ctrl_handler_free(imx219->sd.ctrl_handler);
1014 +       mutex_destroy(&imx219->mutex);
1015 +}
1016 +
1017 +static int imx219_probe(struct i2c_client *client,
1018 +                       const struct i2c_device_id *id)
1019 +{
1020 +       struct device *dev = &client->dev;
1021 +       struct fwnode_handle *endpoint;
1022 +       struct imx219 *imx219;
1023 +       int ret;
1024 +
1025 +       imx219 = devm_kzalloc(&client->dev, sizeof(*imx219), GFP_KERNEL);
1026 +       if (!imx219)
1027 +               return -ENOMEM;
1028 +
1029 +       /* Initialize subdev */
1030 +       v4l2_i2c_subdev_init(&imx219->sd, client, &imx219_subdev_ops);
1031 +
1032 +       /* Get CSI2 bus config */
1033 +       endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(&client->dev),
1034 +                                                 NULL);
1035 +       if (!endpoint) {
1036 +               dev_err(dev, "endpoint node not found\n");
1037 +               return -EINVAL;
1038 +       }
1039 +
1040 +       ret = v4l2_fwnode_endpoint_parse(endpoint, &imx219->ep);
1041 +       fwnode_handle_put(endpoint);
1042 +       if (ret) {
1043 +               dev_err(dev, "Could not parse endpoint\n");
1044 +               return ret;
1045 +       }
1046 +
1047 +       /* Get system clock (xclk) */
1048 +       imx219->xclk = devm_clk_get(dev, "xclk");
1049 +       if (IS_ERR(imx219->xclk)) {
1050 +               dev_err(dev, "failed to get xclk\n");
1051 +               return PTR_ERR(imx219->xclk);
1052 +       }
1053 +
1054 +       imx219->xclk_freq = clk_get_rate(imx219->xclk);
1055 +       if (imx219->xclk_freq != 24000000) {
1056 +               dev_err(dev, "xclk frequency not supported: %d Hz\n",
1057 +                       imx219->xclk_freq);
1058 +               return -EINVAL;
1059 +       }
1060 +
1061 +       ret = imx219_get_regulators(imx219);
1062 +       if (ret)
1063 +               return ret;
1064 +
1065 +       /* request optional power down pin */
1066 +       imx219->xclr_gpio = devm_gpiod_get_optional(dev, "xclr",
1067 +                                                   GPIOD_OUT_HIGH);
1068 +
1069 +       /* Check module identity */
1070 +       ret = imx219_identify_module(imx219);
1071 +       if (ret)
1072 +               return ret;
1073 +
1074 +       /* Set default mode to max resolution */
1075 +       imx219->mode = &supported_modes[0];
1076 +
1077 +       ret = imx219_init_controls(imx219);
1078 +       if (ret)
1079 +               return ret;
1080 +
1081 +       /* Initialize subdev */
1082 +       imx219->sd.internal_ops = &imx219_internal_ops;
1083 +       imx219->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1084 +       imx219->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1085 +
1086 +       /* Initialize source pad */
1087 +       imx219->pad.flags = MEDIA_PAD_FL_SOURCE;
1088 +
1089 +       ret = media_entity_pads_init(&imx219->sd.entity, 1, &imx219->pad);
1090 +       if (ret)
1091 +               goto error_handler_free;
1092 +
1093 +       ret = v4l2_async_register_subdev_sensor_common(&imx219->sd);
1094 +       if (ret < 0)
1095 +               goto error_media_entity;
1096 +
1097 +       pm_runtime_set_active(&client->dev);
1098 +       pm_runtime_enable(&client->dev);
1099 +       pm_runtime_idle(&client->dev);
1100 +
1101 +       return 0;
1102 +
1103 +error_media_entity:
1104 +       media_entity_cleanup(&imx219->sd.entity);
1105 +
1106 +error_handler_free:
1107 +       imx219_free_controls(imx219);
1108 +
1109 +       return ret;
1110 +}
1111 +
1112 +static int imx219_remove(struct i2c_client *client)
1113 +{
1114 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
1115 +       struct imx219 *imx219 = to_imx219(sd);
1116 +
1117 +       v4l2_async_unregister_subdev(sd);
1118 +       media_entity_cleanup(&sd->entity);
1119 +       imx219_free_controls(imx219);
1120 +
1121 +       pm_runtime_disable(&client->dev);
1122 +       pm_runtime_set_suspended(&client->dev);
1123 +
1124 +       return 0;
1125 +}
1126 +
1127 +static const struct of_device_id imx219_dt_ids[] = {
1128 +       { .compatible = "sony,imx219" },
1129 +       { /* sentinel */ }
1130 +};
1131 +MODULE_DEVICE_TABLE(of, imx219_dt_ids);
1132 +
1133 +static struct i2c_driver imx219_i2c_driver = {
1134 +       .driver = {
1135 +               .name = "imx219",
1136 +               .of_match_table = imx219_dt_ids,
1137 +       },
1138 +       .probe = imx219_probe,
1139 +       .remove = imx219_remove,
1140 +};
1141 +
1142 +module_i2c_driver(imx219_i2c_driver);
1143 +
1144 +MODULE_AUTHOR("Dave Stevenson <dave.stevenson@raspberrypi.org");
1145 +MODULE_DESCRIPTION("Sony IMX219 sensor driver");
1146 +MODULE_LICENSE("GPL v2");