target/linux/bcm27xx/patches-5.4/950-0816-media-i2c-Add-IMX290-CMOS-image-sensor-driver.patch

   1 From d13c94482be9ca356df8a04f8fd5f3738dc31ab1 Mon Sep 17 00:00:00 2001
   2 From: Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>
   3 Date: Fri, 4 Oct 2019 13:05:25 -0300
   4 Subject: [PATCH] media: i2c: Add IMX290 CMOS image sensor driver
   5
   6 Commit 828dbc299278065b634e913d2700d254a3224853 upstream.
   7
   8 Add driver for Sony IMX290 CMOS image sensor driver. The driver only
   9 supports I2C interface for programming and MIPI CSI-2 for sensor output.
  10
  11 [Sakari Ailus: Rewrapped a few lines over 80 chars a little.]
  12
  13 Signed-off-by: Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>
  14 Signed-off-by: Sakari Ailus <sakari.ailus@linux.intel.com>
  15 Signed-off-by: Mauro Carvalho Chehab <mchehab+samsung@kernel.org>
  16 ---
  17  drivers/media/i2c/Kconfig  |  11 +
  18  drivers/media/i2c/Makefile |   1 +
  19  drivers/media/i2c/imx290.c | 884 +++++++++++++++++++++++++++++++++++++
  20  3 files changed, 896 insertions(+)
  21  create mode 100644 drivers/media/i2c/imx290.c
  22
  23 --- a/drivers/media/i2c/Kconfig
  24 +++ b/drivers/media/i2c/Kconfig
  25 @@ -609,6 +609,17 @@ config VIDEO_IMX274
  26           This is a V4L2 sensor driver for the Sony IMX274
  27           CMOS image sensor.
  28
  29 +config VIDEO_IMX290
  30 +       tristate "Sony IMX290 sensor support"
  31 +       depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
  32 +       select V4L2_FWNODE
  33 +       help
  34 +         This is a Video4Linux2 sensor driver for the Sony
  35 +         IMX290 camera sensor.
  36 +
  37 +         To compile this driver as a module, choose M here: the
  38 +         module will be called imx290.
  39 +
  40  config VIDEO_IMX477
  41         tristate "Sony IMX477 sensor support"
  42         depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
  43 --- a/drivers/media/i2c/Makefile
  44 +++ b/drivers/media/i2c/Makefile
  45 @@ -114,6 +114,7 @@ obj-$(CONFIG_VIDEO_IMX214)  += imx214.o
  46  obj-$(CONFIG_VIDEO_IMX219)     += imx219.o
  47  obj-$(CONFIG_VIDEO_IMX258)     += imx258.o
  48  obj-$(CONFIG_VIDEO_IMX274)     += imx274.o
  49 +obj-$(CONFIG_VIDEO_IMX290)     += imx290.o
  50  obj-$(CONFIG_VIDEO_IMX477)     += imx477.o
  51  obj-$(CONFIG_VIDEO_IMX319)     += imx319.o
  52  obj-$(CONFIG_VIDEO_IMX355)     += imx355.o
  53 --- /dev/null
  54 +++ b/drivers/media/i2c/imx290.c
  55 @@ -0,0 +1,884 @@
  56 +// SPDX-License-Identifier: GPL-2.0
  57 +/*
  58 + * Sony IMX290 CMOS Image Sensor Driver
  59 + *
  60 + * Copyright (C) 2019 FRAMOS GmbH.
  61 + *
  62 + * Copyright (C) 2019 Linaro Ltd.
  63 + * Author: Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>
  64 + */
  65 +
  66 +#include <linux/clk.h>
  67 +#include <linux/delay.h>
  68 +#include <linux/gpio/consumer.h>
  69 +#include <linux/i2c.h>
  70 +#include <linux/module.h>
  71 +#include <linux/pm_runtime.h>
  72 +#include <linux/regmap.h>
  73 +#include <linux/regulator/consumer.h>
  74 +#include <media/media-entity.h>
  75 +#include <media/v4l2-ctrls.h>
  76 +#include <media/v4l2-device.h>
  77 +#include <media/v4l2-fwnode.h>
  78 +#include <media/v4l2-subdev.h>
  79 +
  80 +#define IMX290_STANDBY 0x3000
  81 +#define IMX290_REGHOLD 0x3001
  82 +#define IMX290_XMSTA 0x3002
  83 +#define IMX290_GAIN 0x3014
  84 +
  85 +#define IMX290_DEFAULT_LINK_FREQ 445500000
  86 +
  87 +static const char * const imx290_supply_name[] = {
  88 +       "vdda",
  89 +       "vddd",
  90 +       "vdddo",
  91 +};
  92 +
  93 +#define IMX290_NUM_SUPPLIES ARRAY_SIZE(imx290_supply_name)
  94 +
  95 +struct imx290_regval {
  96 +       u16 reg;
  97 +       u8 val;
  98 +};
  99 +
 100 +struct imx290_mode {
 101 +       u32 width;
 102 +       u32 height;
 103 +       u32 pixel_rate;
 104 +       u32 link_freq_index;
 105 +
 106 +       const struct imx290_regval *data;
 107 +       u32 data_size;
 108 +};
 109 +
 110 +struct imx290 {
 111 +       struct device *dev;
 112 +       struct clk *xclk;
 113 +       struct regmap *regmap;
 114 +
 115 +       struct v4l2_subdev sd;
 116 +       struct v4l2_fwnode_endpoint ep;
 117 +       struct media_pad pad;
 118 +       struct v4l2_mbus_framefmt current_format;
 119 +       const struct imx290_mode *current_mode;
 120 +
 121 +       struct regulator_bulk_data supplies[IMX290_NUM_SUPPLIES];
 122 +       struct gpio_desc *rst_gpio;
 123 +
 124 +       struct v4l2_ctrl_handler ctrls;
 125 +       struct v4l2_ctrl *link_freq;
 126 +       struct v4l2_ctrl *pixel_rate;
 127 +
 128 +       struct mutex lock;
 129 +};
 130 +
 131 +struct imx290_pixfmt {
 132 +       u32 code;
 133 +};
 134 +
 135 +static const struct imx290_pixfmt imx290_formats[] = {
 136 +       { MEDIA_BUS_FMT_SRGGB10_1X10 },
 137 +};
 138 +
 139 +static const struct regmap_config imx290_regmap_config = {
 140 +       .reg_bits = 16,
 141 +       .val_bits = 8,
 142 +       .cache_type = REGCACHE_RBTREE,
 143 +};
 144 +
 145 +static const struct imx290_regval imx290_global_init_settings[] = {
 146 +       { 0x3007, 0x00 },
 147 +       { 0x3009, 0x00 },
 148 +       { 0x3018, 0x65 },
 149 +       { 0x3019, 0x04 },
 150 +       { 0x301a, 0x00 },
 151 +       { 0x3443, 0x03 },
 152 +       { 0x3444, 0x20 },
 153 +       { 0x3445, 0x25 },
 154 +       { 0x3407, 0x03 },
 155 +       { 0x303a, 0x0c },
 156 +       { 0x3040, 0x00 },
 157 +       { 0x3041, 0x00 },
 158 +       { 0x303c, 0x00 },
 159 +       { 0x303d, 0x00 },
 160 +       { 0x3042, 0x9c },
 161 +       { 0x3043, 0x07 },
 162 +       { 0x303e, 0x49 },
 163 +       { 0x303f, 0x04 },
 164 +       { 0x304b, 0x0a },
 165 +       { 0x300f, 0x00 },
 166 +       { 0x3010, 0x21 },
 167 +       { 0x3012, 0x64 },
 168 +       { 0x3016, 0x09 },
 169 +       { 0x3070, 0x02 },
 170 +       { 0x3071, 0x11 },
 171 +       { 0x309b, 0x10 },
 172 +       { 0x309c, 0x22 },
 173 +       { 0x30a2, 0x02 },
 174 +       { 0x30a6, 0x20 },
 175 +       { 0x30a8, 0x20 },
 176 +       { 0x30aa, 0x20 },
 177 +       { 0x30ac, 0x20 },
 178 +       { 0x30b0, 0x43 },
 179 +       { 0x3119, 0x9e },
 180 +       { 0x311c, 0x1e },
 181 +       { 0x311e, 0x08 },
 182 +       { 0x3128, 0x05 },
 183 +       { 0x313d, 0x83 },
 184 +       { 0x3150, 0x03 },
 185 +       { 0x317e, 0x00 },
 186 +       { 0x32b8, 0x50 },
 187 +       { 0x32b9, 0x10 },
 188 +       { 0x32ba, 0x00 },
 189 +       { 0x32bb, 0x04 },
 190 +       { 0x32c8, 0x50 },
 191 +       { 0x32c9, 0x10 },
 192 +       { 0x32ca, 0x00 },
 193 +       { 0x32cb, 0x04 },
 194 +       { 0x332c, 0xd3 },
 195 +       { 0x332d, 0x10 },
 196 +       { 0x332e, 0x0d },
 197 +       { 0x3358, 0x06 },
 198 +       { 0x3359, 0xe1 },
 199 +       { 0x335a, 0x11 },
 200 +       { 0x3360, 0x1e },
 201 +       { 0x3361, 0x61 },
 202 +       { 0x3362, 0x10 },
 203 +       { 0x33b0, 0x50 },
 204 +       { 0x33b2, 0x1a },
 205 +       { 0x33b3, 0x04 },
 206 +};
 207 +
 208 +static const struct imx290_regval imx290_1080p_settings[] = {
 209 +       /* mode settings */
 210 +       { 0x3007, 0x00 },
 211 +       { 0x303a, 0x0c },
 212 +       { 0x3414, 0x0a },
 213 +       { 0x3472, 0x80 },
 214 +       { 0x3473, 0x07 },
 215 +       { 0x3418, 0x38 },
 216 +       { 0x3419, 0x04 },
 217 +       { 0x3012, 0x64 },
 218 +       { 0x3013, 0x00 },
 219 +       { 0x305c, 0x18 },
 220 +       { 0x305d, 0x03 },
 221 +       { 0x305e, 0x20 },
 222 +       { 0x305f, 0x01 },
 223 +       { 0x315e, 0x1a },
 224 +       { 0x3164, 0x1a },
 225 +       { 0x3480, 0x49 },
 226 +       /* data rate settings */
 227 +       { 0x3009, 0x01 },
 228 +       { 0x3405, 0x10 },
 229 +       { 0x3446, 0x57 },
 230 +       { 0x3447, 0x00 },
 231 +       { 0x3448, 0x37 },
 232 +       { 0x3449, 0x00 },
 233 +       { 0x344a, 0x1f },
 234 +       { 0x344b, 0x00 },
 235 +       { 0x344c, 0x1f },
 236 +       { 0x344d, 0x00 },
 237 +       { 0x344e, 0x1f },
 238 +       { 0x344f, 0x00 },
 239 +       { 0x3450, 0x77 },
 240 +       { 0x3451, 0x00 },
 241 +       { 0x3452, 0x1f },
 242 +       { 0x3453, 0x00 },
 243 +       { 0x3454, 0x17 },
 244 +       { 0x3455, 0x00 },
 245 +       { 0x301c, 0x98 },
 246 +       { 0x301d, 0x08 },
 247 +};
 248 +
 249 +static const struct imx290_regval imx290_720p_settings[] = {
 250 +       /* mode settings */
 251 +       { 0x3007, 0x10 },
 252 +       { 0x303a, 0x06 },
 253 +       { 0x3414, 0x04 },
 254 +       { 0x3472, 0x00 },
 255 +       { 0x3473, 0x05 },
 256 +       { 0x3418, 0xd0 },
 257 +       { 0x3419, 0x02 },
 258 +       { 0x3012, 0x64 },
 259 +       { 0x3013, 0x00 },
 260 +       { 0x305c, 0x20 },
 261 +       { 0x305d, 0x00 },
 262 +       { 0x305e, 0x20 },
 263 +       { 0x305f, 0x01 },
 264 +       { 0x315e, 0x1a },
 265 +       { 0x3164, 0x1a },
 266 +       { 0x3480, 0x49 },
 267 +       /* data rate settings */
 268 +       { 0x3009, 0x01 },
 269 +       { 0x3405, 0x10 },
 270 +       { 0x3446, 0x4f },
 271 +       { 0x3447, 0x00 },
 272 +       { 0x3448, 0x2f },
 273 +       { 0x3449, 0x00 },
 274 +       { 0x344a, 0x17 },
 275 +       { 0x344b, 0x00 },
 276 +       { 0x344c, 0x17 },
 277 +       { 0x344d, 0x00 },
 278 +       { 0x344e, 0x17 },
 279 +       { 0x344f, 0x00 },
 280 +       { 0x3450, 0x57 },
 281 +       { 0x3451, 0x00 },
 282 +       { 0x3452, 0x17 },
 283 +       { 0x3453, 0x00 },
 284 +       { 0x3454, 0x17 },
 285 +       { 0x3455, 0x00 },
 286 +       { 0x301c, 0xe4 },
 287 +       { 0x301d, 0x0c },
 288 +};
 289 +
 290 +static const struct imx290_regval imx290_10bit_settings[] = {
 291 +       { 0x3005, 0x00},
 292 +       { 0x3046, 0x00},
 293 +       { 0x3129, 0x1d},
 294 +       { 0x317c, 0x12},
 295 +       { 0x31ec, 0x37},
 296 +       { 0x3441, 0x0a},
 297 +       { 0x3442, 0x0a},
 298 +       { 0x300a, 0x3c},
 299 +       { 0x300b, 0x00},
 300 +};
 301 +
 302 +/* supported link frequencies */
 303 +static const s64 imx290_link_freq[] = {
 304 +       IMX290_DEFAULT_LINK_FREQ,
 305 +};
 306 +
 307 +/* Mode configs */
 308 +static const struct imx290_mode imx290_modes[] = {
 309 +       {
 310 +               .width = 1920,
 311 +               .height = 1080,
 312 +               .data = imx290_1080p_settings,
 313 +               .data_size = ARRAY_SIZE(imx290_1080p_settings),
 314 +               .pixel_rate = 178200000,
 315 +               .link_freq_index = 0,
 316 +       },
 317 +       {
 318 +               .width = 1280,
 319 +               .height = 720,
 320 +               .data = imx290_720p_settings,
 321 +               .data_size = ARRAY_SIZE(imx290_720p_settings),
 322 +               .pixel_rate = 178200000,
 323 +               .link_freq_index = 0,
 324 +       },
 325 +};
 326 +
 327 +static inline struct imx290 *to_imx290(struct v4l2_subdev *_sd)
 328 +{
 329 +       return container_of(_sd, struct imx290, sd);
 330 +}
 331 +
 332 +static inline int imx290_read_reg(struct imx290 *imx290, u16 addr, u8 *value)
 333 +{
 334 +       unsigned int regval;
 335 +       int ret;
 336 +
 337 +       ret = regmap_read(imx290->regmap, addr, &regval);
 338 +       if (ret) {
 339 +               dev_err(imx290->dev, "I2C read failed for addr: %x\n", addr);
 340 +               return ret;
 341 +       }
 342 +
 343 +       *value = regval & 0xff;
 344 +
 345 +       return 0;
 346 +}
 347 +
 348 +static int imx290_write_reg(struct imx290 *imx290, u16 addr, u8 value)
 349 +{
 350 +       int ret;
 351 +
 352 +       ret = regmap_write(imx290->regmap, addr, value);
 353 +       if (ret) {
 354 +               dev_err(imx290->dev, "I2C write failed for addr: %x\n", addr);
 355 +               return ret;
 356 +       }
 357 +
 358 +       return ret;
 359 +}
 360 +
 361 +static int imx290_set_register_array(struct imx290 *imx290,
 362 +                                    const struct imx290_regval *settings,
 363 +                                    unsigned int num_settings)
 364 +{
 365 +       unsigned int i;
 366 +       int ret;
 367 +
 368 +       for (i = 0; i < num_settings; ++i, ++settings) {
 369 +               ret = imx290_write_reg(imx290, settings->reg, settings->val);
 370 +               if (ret < 0)
 371 +                       return ret;
 372 +
 373 +               /* Settle time is 10ms for all registers */
 374 +               msleep(10);
 375 +       }
 376 +
 377 +       return 0;
 378 +}
 379 +
 380 +static int imx290_write_buffered_reg(struct imx290 *imx290, u16 address_low,
 381 +                                    u8 nr_regs, u32 value)
 382 +{
 383 +       unsigned int i;
 384 +       int ret;
 385 +
 386 +       ret = imx290_write_reg(imx290, IMX290_REGHOLD, 0x01);
 387 +       if (ret) {
 388 +               dev_err(imx290->dev, "Error setting hold register\n");
 389 +               return ret;
 390 +       }
 391 +
 392 +       for (i = 0; i < nr_regs; i++) {
 393 +               ret = imx290_write_reg(imx290, address_low + i,
 394 +                                      (u8)(value >> (i * 8)));
 395 +               if (ret) {
 396 +                       dev_err(imx290->dev, "Error writing buffered registers\n");
 397 +                       return ret;
 398 +               }
 399 +       }
 400 +
 401 +       ret = imx290_write_reg(imx290, IMX290_REGHOLD, 0x00);
 402 +       if (ret) {
 403 +               dev_err(imx290->dev, "Error setting hold register\n");
 404 +               return ret;
 405 +       }
 406 +
 407 +       return ret;
 408 +}
 409 +
 410 +static int imx290_set_gain(struct imx290 *imx290, u32 value)
 411 +{
 412 +       int ret;
 413 +
 414 +       ret = imx290_write_buffered_reg(imx290, IMX290_GAIN, 1, value);
 415 +       if (ret)
 416 +               dev_err(imx290->dev, "Unable to write gain\n");
 417 +
 418 +       return ret;
 419 +}
 420 +
 421 +/* Stop streaming */
 422 +static int imx290_stop_streaming(struct imx290 *imx290)
 423 +{
 424 +       int ret;
 425 +
 426 +       ret = imx290_write_reg(imx290, IMX290_STANDBY, 0x01);
 427 +       if (ret < 0)
 428 +               return ret;
 429 +
 430 +       msleep(30);
 431 +
 432 +       return imx290_write_reg(imx290, IMX290_XMSTA, 0x01);
 433 +}
 434 +
 435 +static int imx290_set_ctrl(struct v4l2_ctrl *ctrl)
 436 +{
 437 +       struct imx290 *imx290 = container_of(ctrl->handler,
 438 +                                            struct imx290, ctrls);
 439 +       int ret = 0;
 440 +
 441 +       /* V4L2 controls values will be applied only when power is already up */
 442 +       if (!pm_runtime_get_if_in_use(imx290->dev))
 443 +               return 0;
 444 +
 445 +       switch (ctrl->id) {
 446 +       case V4L2_CID_GAIN:
 447 +               ret = imx290_set_gain(imx290, ctrl->val);
 448 +               break;
 449 +       default:
 450 +               ret = -EINVAL;
 451 +               break;
 452 +       }
 453 +
 454 +       pm_runtime_put(imx290->dev);
 455 +
 456 +       return ret;
 457 +}
 458 +
 459 +static const struct v4l2_ctrl_ops imx290_ctrl_ops = {
 460 +       .s_ctrl = imx290_set_ctrl,
 461 +};
 462 +
 463 +static int imx290_enum_mbus_code(struct v4l2_subdev *sd,
 464 +                                struct v4l2_subdev_pad_config *cfg,
 465 +                                struct v4l2_subdev_mbus_code_enum *code)
 466 +{
 467 +       if (code->index >= ARRAY_SIZE(imx290_formats))
 468 +               return -EINVAL;
 469 +
 470 +       code->code = imx290_formats[code->index].code;
 471 +
 472 +       return 0;
 473 +}
 474 +
 475 +static int imx290_get_fmt(struct v4l2_subdev *sd,
 476 +                         struct v4l2_subdev_pad_config *cfg,
 477 +                         struct v4l2_subdev_format *fmt)
 478 +{
 479 +       struct imx290 *imx290 = to_imx290(sd);
 480 +       struct v4l2_mbus_framefmt *framefmt;
 481 +
 482 +       mutex_lock(&imx290->lock);
 483 +
 484 +       if (fmt->which == V4L2_SUBDEV_FORMAT_TRY)
 485 +               framefmt = v4l2_subdev_get_try_format(&imx290->sd, cfg,
 486 +                                                     fmt->pad);
 487 +       else
 488 +               framefmt = &imx290->current_format;
 489 +
 490 +       fmt->format = *framefmt;
 491 +
 492 +       mutex_unlock(&imx290->lock);
 493 +
 494 +       return 0;
 495 +}
 496 +
 497 +static int imx290_set_fmt(struct v4l2_subdev *sd,
 498 +                         struct v4l2_subdev_pad_config *cfg,
 499 +                     struct v4l2_subdev_format *fmt)
 500 +{
 501 +       struct imx290 *imx290 = to_imx290(sd);
 502 +       const struct imx290_mode *mode;
 503 +       struct v4l2_mbus_framefmt *format;
 504 +       unsigned int i;
 505 +
 506 +       mutex_lock(&imx290->lock);
 507 +
 508 +       mode = v4l2_find_nearest_size(imx290_modes,
 509 +                                     ARRAY_SIZE(imx290_modes),
 510 +                                     width, height,
 511 +                                     fmt->format.width, fmt->format.height);
 512 +
 513 +       fmt->format.width = mode->width;
 514 +       fmt->format.height = mode->height;
 515 +
 516 +       for (i = 0; i < ARRAY_SIZE(imx290_formats); i++)
 517 +               if (imx290_formats[i].code == fmt->format.code)
 518 +                       break;
 519 +
 520 +       if (i >= ARRAY_SIZE(imx290_formats))
 521 +               i = 0;
 522 +
 523 +       fmt->format.code = imx290_formats[i].code;
 524 +       fmt->format.field = V4L2_FIELD_NONE;
 525 +
 526 +       if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) {
 527 +               format = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
 528 +       } else {
 529 +               format = &imx290->current_format;
 530 +               __v4l2_ctrl_s_ctrl(imx290->link_freq, mode->link_freq_index);
 531 +               __v4l2_ctrl_s_ctrl_int64(imx290->pixel_rate, mode->pixel_rate);
 532 +
 533 +               imx290->current_mode = mode;
 534 +       }
 535 +
 536 +       *format = fmt->format;
 537 +
 538 +       mutex_unlock(&imx290->lock);
 539 +
 540 +       return 0;
 541 +}
 542 +
 543 +static int imx290_entity_init_cfg(struct v4l2_subdev *subdev,
 544 +                                 struct v4l2_subdev_pad_config *cfg)
 545 +{
 546 +       struct v4l2_subdev_format fmt = { 0 };
 547 +
 548 +       fmt.which = cfg ? V4L2_SUBDEV_FORMAT_TRY : V4L2_SUBDEV_FORMAT_ACTIVE;
 549 +       fmt.format.width = 1920;
 550 +       fmt.format.height = 1080;
 551 +
 552 +       imx290_set_fmt(subdev, cfg, &fmt);
 553 +
 554 +       return 0;
 555 +}
 556 +
 557 +static int imx290_write_current_format(struct imx290 *imx290,
 558 +                                      struct v4l2_mbus_framefmt *format)
 559 +{
 560 +       int ret;
 561 +
 562 +       switch (format->code) {
 563 +       case MEDIA_BUS_FMT_SRGGB10_1X10:
 564 +               ret = imx290_set_register_array(imx290, imx290_10bit_settings,
 565 +                                               ARRAY_SIZE(
 566 +                                                       imx290_10bit_settings));
 567 +               if (ret < 0) {
 568 +                       dev_err(imx290->dev, "Could not set format registers\n");
 569 +                       return ret;
 570 +               }
 571 +               break;
 572 +       default:
 573 +               dev_err(imx290->dev, "Unknown pixel format\n");
 574 +               return -EINVAL;
 575 +       }
 576 +
 577 +       return 0;
 578 +}
 579 +
 580 +/* Start streaming */
 581 +static int imx290_start_streaming(struct imx290 *imx290)
 582 +{
 583 +       int ret;
 584 +
 585 +       /* Set init register settings */
 586 +       ret = imx290_set_register_array(imx290, imx290_global_init_settings,
 587 +                                       ARRAY_SIZE(
 588 +                                               imx290_global_init_settings));
 589 +       if (ret < 0) {
 590 +               dev_err(imx290->dev, "Could not set init registers\n");
 591 +               return ret;
 592 +       }
 593 +
 594 +       /* Set current frame format */
 595 +       ret = imx290_write_current_format(imx290, &imx290->current_format);
 596 +       if (ret < 0) {
 597 +               dev_err(imx290->dev, "Could not set frame format\n");
 598 +               return ret;
 599 +       }
 600 +
 601 +       /* Apply default values of current mode */
 602 +       ret = imx290_set_register_array(imx290, imx290->current_mode->data,
 603 +                                       imx290->current_mode->data_size);
 604 +       if (ret < 0) {
 605 +               dev_err(imx290->dev, "Could not set current mode\n");
 606 +               return ret;
 607 +       }
 608 +
 609 +       /* Apply customized values from user */
 610 +       ret = v4l2_ctrl_handler_setup(imx290->sd.ctrl_handler);
 611 +       if (ret) {
 612 +               dev_err(imx290->dev, "Could not sync v4l2 controls\n");
 613 +               return ret;
 614 +       }
 615 +
 616 +       ret = imx290_write_reg(imx290, IMX290_STANDBY, 0x00);
 617 +       if (ret < 0)
 618 +               return ret;
 619 +
 620 +       msleep(30);
 621 +
 622 +       /* Start streaming */
 623 +       return imx290_write_reg(imx290, IMX290_XMSTA, 0x00);
 624 +}
 625 +
 626 +static int imx290_set_stream(struct v4l2_subdev *sd, int enable)
 627 +{
 628 +       struct imx290 *imx290 = to_imx290(sd);
 629 +       int ret = 0;
 630 +
 631 +       if (enable) {
 632 +               ret = pm_runtime_get_sync(imx290->dev);
 633 +               if (ret < 0) {
 634 +                       pm_runtime_put_noidle(imx290->dev);
 635 +                       goto unlock_and_return;
 636 +               }
 637 +
 638 +               ret = imx290_start_streaming(imx290);
 639 +               if (ret) {
 640 +                       dev_err(imx290->dev, "Start stream failed\n");
 641 +                       pm_runtime_put(imx290->dev);
 642 +                       goto unlock_and_return;
 643 +               }
 644 +       } else {
 645 +               imx290_stop_streaming(imx290);
 646 +               pm_runtime_put(imx290->dev);
 647 +       }
 648 +
 649 +unlock_and_return:
 650 +
 651 +       return ret;
 652 +}
 653 +
 654 +static int imx290_get_regulators(struct device *dev, struct imx290 *imx290)
 655 +{
 656 +       unsigned int i;
 657 +
 658 +       for (i = 0; i < IMX290_NUM_SUPPLIES; i++)
 659 +               imx290->supplies[i].supply = imx290_supply_name[i];
 660 +
 661 +       return devm_regulator_bulk_get(dev, IMX290_NUM_SUPPLIES,
 662 +                                      imx290->supplies);
 663 +}
 664 +
 665 +static int imx290_power_on(struct device *dev)
 666 +{
 667 +       struct i2c_client *client = to_i2c_client(dev);
 668 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
 669 +       struct imx290 *imx290 = to_imx290(sd);
 670 +       int ret;
 671 +
 672 +       ret = clk_prepare_enable(imx290->xclk);
 673 +       if (ret) {
 674 +               dev_err(imx290->dev, "Failed to enable clock\n");
 675 +               return ret;
 676 +       }
 677 +
 678 +       ret = regulator_bulk_enable(IMX290_NUM_SUPPLIES, imx290->supplies);
 679 +       if (ret) {
 680 +               dev_err(imx290->dev, "Failed to enable regulators\n");
 681 +               clk_disable_unprepare(imx290->xclk);
 682 +               return ret;
 683 +       }
 684 +
 685 +       usleep_range(1, 2);
 686 +       gpiod_set_value_cansleep(imx290->rst_gpio, 1);
 687 +       usleep_range(30000, 31000);
 688 +
 689 +       return 0;
 690 +}
 691 +
 692 +static int imx290_power_off(struct device *dev)
 693 +{
 694 +       struct i2c_client *client = to_i2c_client(dev);
 695 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
 696 +       struct imx290 *imx290 = to_imx290(sd);
 697 +
 698 +       clk_disable_unprepare(imx290->xclk);
 699 +       gpiod_set_value_cansleep(imx290->rst_gpio, 0);
 700 +       regulator_bulk_disable(IMX290_NUM_SUPPLIES, imx290->supplies);
 701 +
 702 +       return 0;
 703 +}
 704 +
 705 +static const struct dev_pm_ops imx290_pm_ops = {
 706 +       SET_RUNTIME_PM_OPS(imx290_power_on, imx290_power_off, NULL)
 707 +};
 708 +
 709 +static const struct v4l2_subdev_video_ops imx290_video_ops = {
 710 +       .s_stream = imx290_set_stream,
 711 +};
 712 +
 713 +static const struct v4l2_subdev_pad_ops imx290_pad_ops = {
 714 +       .init_cfg = imx290_entity_init_cfg,
 715 +       .enum_mbus_code = imx290_enum_mbus_code,
 716 +       .get_fmt = imx290_get_fmt,
 717 +       .set_fmt = imx290_set_fmt,
 718 +};
 719 +
 720 +static const struct v4l2_subdev_ops imx290_subdev_ops = {
 721 +       .video = &imx290_video_ops,
 722 +       .pad = &imx290_pad_ops,
 723 +};
 724 +
 725 +static const struct media_entity_operations imx290_subdev_entity_ops = {
 726 +       .link_validate = v4l2_subdev_link_validate,
 727 +};
 728 +
 729 +static int imx290_probe(struct i2c_client *client)
 730 +{
 731 +       struct device *dev = &client->dev;
 732 +       struct fwnode_handle *endpoint;
 733 +       struct imx290 *imx290;
 734 +       u32 xclk_freq;
 735 +       int ret;
 736 +
 737 +       imx290 = devm_kzalloc(dev, sizeof(*imx290), GFP_KERNEL);
 738 +       if (!imx290)
 739 +               return -ENOMEM;
 740 +
 741 +       imx290->dev = dev;
 742 +       imx290->regmap = devm_regmap_init_i2c(client, &imx290_regmap_config);
 743 +       if (IS_ERR(imx290->regmap)) {
 744 +               dev_err(dev, "Unable to initialize I2C\n");
 745 +               return -ENODEV;
 746 +       }
 747 +
 748 +       endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(dev), NULL);
 749 +       if (!endpoint) {
 750 +               dev_err(dev, "Endpoint node not found\n");
 751 +               return -EINVAL;
 752 +       }
 753 +
 754 +       ret = v4l2_fwnode_endpoint_alloc_parse(endpoint, &imx290->ep);
 755 +       fwnode_handle_put(endpoint);
 756 +       if (ret) {
 757 +               dev_err(dev, "Parsing endpoint node failed\n");
 758 +               goto free_err;
 759 +       }
 760 +
 761 +       if (!imx290->ep.nr_of_link_frequencies) {
 762 +               dev_err(dev, "link-frequency property not found in DT\n");
 763 +               ret = -EINVAL;
 764 +               goto free_err;
 765 +       }
 766 +
 767 +       if (imx290->ep.link_frequencies[0] != IMX290_DEFAULT_LINK_FREQ) {
 768 +               dev_err(dev, "Unsupported link frequency\n");
 769 +               ret = -EINVAL;
 770 +               goto free_err;
 771 +       }
 772 +
 773 +       /* Only CSI2 is supported for now */
 774 +       if (imx290->ep.bus_type != V4L2_MBUS_CSI2_DPHY) {
 775 +               dev_err(dev, "Unsupported bus type, should be CSI2\n");
 776 +               ret = -EINVAL;
 777 +               goto free_err;
 778 +       }
 779 +
 780 +       /* Set default mode to max resolution */
 781 +       imx290->current_mode = &imx290_modes[0];
 782 +
 783 +       /* get system clock (xclk) */
 784 +       imx290->xclk = devm_clk_get(dev, "xclk");
 785 +       if (IS_ERR(imx290->xclk)) {
 786 +               dev_err(dev, "Could not get xclk");
 787 +               ret = PTR_ERR(imx290->xclk);
 788 +               goto free_err;
 789 +       }
 790 +
 791 +       ret = fwnode_property_read_u32(dev_fwnode(dev), "clock-frequency",
 792 +                                      &xclk_freq);
 793 +       if (ret) {
 794 +               dev_err(dev, "Could not get xclk frequency\n");
 795 +               goto free_err;
 796 +       }
 797 +
 798 +       /* external clock must be 37.125 MHz */
 799 +       if (xclk_freq != 37125000) {
 800 +               dev_err(dev, "External clock frequency %u is not supported\n",
 801 +                       xclk_freq);
 802 +               ret = -EINVAL;
 803 +               goto free_err;
 804 +       }
 805 +
 806 +       ret = clk_set_rate(imx290->xclk, xclk_freq);
 807 +       if (ret) {
 808 +               dev_err(dev, "Could not set xclk frequency\n");
 809 +               goto free_err;
 810 +       }
 811 +
 812 +       ret = imx290_get_regulators(dev, imx290);
 813 +       if (ret < 0) {
 814 +               dev_err(dev, "Cannot get regulators\n");
 815 +               goto free_err;
 816 +       }
 817 +
 818 +       imx290->rst_gpio = devm_gpiod_get_optional(dev, "reset", GPIOD_ASIS);
 819 +       if (IS_ERR(imx290->rst_gpio)) {
 820 +               dev_err(dev, "Cannot get reset gpio\n");
 821 +               ret = PTR_ERR(imx290->rst_gpio);
 822 +               goto free_err;
 823 +       }
 824 +
 825 +       mutex_init(&imx290->lock);
 826 +
 827 +       v4l2_ctrl_handler_init(&imx290->ctrls, 3);
 828 +
 829 +       v4l2_ctrl_new_std(&imx290->ctrls, &imx290_ctrl_ops,
 830 +                         V4L2_CID_GAIN, 0, 72, 1, 0);
 831 +       imx290->link_freq =
 832 +               v4l2_ctrl_new_int_menu(&imx290->ctrls,
 833 +                                      &imx290_ctrl_ops,
 834 +                                      V4L2_CID_LINK_FREQ,
 835 +                                      ARRAY_SIZE(imx290_link_freq) - 1,
 836 +                                      0, imx290_link_freq);
 837 +       if (imx290->link_freq)
 838 +               imx290->link_freq->flags |= V4L2_CTRL_FLAG_READ_ONLY;
 839 +
 840 +       imx290->pixel_rate = v4l2_ctrl_new_std(&imx290->ctrls, &imx290_ctrl_ops,
 841 +                                              V4L2_CID_PIXEL_RATE, 1,
 842 +                                              INT_MAX, 1,
 843 +                                              imx290_modes[0].pixel_rate);
 844 +
 845 +       imx290->sd.ctrl_handler = &imx290->ctrls;
 846 +
 847 +       if (imx290->ctrls.error) {
 848 +               dev_err(dev, "Control initialization error %d\n",
 849 +                       imx290->ctrls.error);
 850 +               ret = imx290->ctrls.error;
 851 +               goto free_ctrl;
 852 +       }
 853 +
 854 +       v4l2_i2c_subdev_init(&imx290->sd, client, &imx290_subdev_ops);
 855 +       imx290->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 856 +       imx290->sd.dev = &client->dev;
 857 +       imx290->sd.entity.ops = &imx290_subdev_entity_ops;
 858 +       imx290->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
 859 +
 860 +       imx290->pad.flags = MEDIA_PAD_FL_SOURCE;
 861 +       ret = media_entity_pads_init(&imx290->sd.entity, 1, &imx290->pad);
 862 +       if (ret < 0) {
 863 +               dev_err(dev, "Could not register media entity\n");
 864 +               goto free_ctrl;
 865 +       }
 866 +
 867 +       ret = v4l2_async_register_subdev(&imx290->sd);
 868 +       if (ret < 0) {
 869 +               dev_err(dev, "Could not register v4l2 device\n");
 870 +               goto free_entity;
 871 +       }
 872 +
 873 +       /* Power on the device to match runtime PM state below */
 874 +       ret = imx290_power_on(dev);
 875 +       if (ret < 0) {
 876 +               dev_err(dev, "Could not power on the device\n");
 877 +               goto free_entity;
 878 +       }
 879 +
 880 +       pm_runtime_set_active(dev);
 881 +       pm_runtime_enable(dev);
 882 +       pm_runtime_idle(dev);
 883 +
 884 +       v4l2_fwnode_endpoint_free(&imx290->ep);
 885 +
 886 +       return 0;
 887 +
 888 +free_entity:
 889 +       media_entity_cleanup(&imx290->sd.entity);
 890 +free_ctrl:
 891 +       v4l2_ctrl_handler_free(&imx290->ctrls);
 892 +       mutex_destroy(&imx290->lock);
 893 +free_err:
 894 +       v4l2_fwnode_endpoint_free(&imx290->ep);
 895 +
 896 +       return ret;
 897 +}
 898 +
 899 +static int imx290_remove(struct i2c_client *client)
 900 +{
 901 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
 902 +       struct imx290 *imx290 = to_imx290(sd);
 903 +
 904 +       v4l2_async_unregister_subdev(sd);
 905 +       media_entity_cleanup(&sd->entity);
 906 +       v4l2_ctrl_handler_free(sd->ctrl_handler);
 907 +
 908 +       mutex_destroy(&imx290->lock);
 909 +
 910 +       pm_runtime_disable(imx290->dev);
 911 +       if (!pm_runtime_status_suspended(imx290->dev))
 912 +               imx290_power_off(imx290->dev);
 913 +       pm_runtime_set_suspended(imx290->dev);
 914 +
 915 +       return 0;
 916 +}
 917 +
 918 +static const struct of_device_id imx290_of_match[] = {
 919 +       { .compatible = "sony,imx290" },
 920 +       { /* sentinel */ }
 921 +};
 922 +MODULE_DEVICE_TABLE(of, imx290_of_match);
 923 +
 924 +static struct i2c_driver imx290_i2c_driver = {
 925 +       .probe_new  = imx290_probe,
 926 +       .remove = imx290_remove,
 927 +       .driver = {
 928 +               .name  = "imx290",
 929 +               .pm = &imx290_pm_ops,
 930 +               .of_match_table = of_match_ptr(imx290_of_match),
 931 +       },
 932 +};
 933 +
 934 +module_i2c_driver(imx290_i2c_driver);
 935 +
 936 +MODULE_DESCRIPTION("Sony IMX290 CMOS Image Sensor Driver");
 937 +MODULE_AUTHOR("FRAMOS GmbH");
 938 +MODULE_AUTHOR("Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>");
 939 +MODULE_LICENSE("GPL v2");