target/linux/bcm27xx/patches-5.15/950-0885-media-i2c-Add-driver-of-Arducam-Pivariety-series-cam.patch

   1 From 94203b7e5bbb814c1d18fc5d95fea18b66041c03 Mon Sep 17 00:00:00 2001
   2 From: Lee Jackson <info@arducam.com>
   3 Date: Thu, 14 Apr 2022 17:31:01 +0800
   4 Subject: [PATCH] media: i2c: Add driver of Arducam Pivariety series
   5  camera
   6
   7 Add a driver for the Arducam Pivariety series CSI2 camera sensor.
   8
   9 Signed-off-by: Lee Jackson <info@arducam.com>
  10 ---
  11  drivers/media/i2c/Kconfig             |   11 +
  12  drivers/media/i2c/Makefile            |    1 +
  13  drivers/media/i2c/arducam-pivariety.c | 1469 +++++++++++++++++++++++++
  14  drivers/media/i2c/arducam-pivariety.h |  107 ++
  15  4 files changed, 1588 insertions(+)
  16  create mode 100644 drivers/media/i2c/arducam-pivariety.c
  17  create mode 100644 drivers/media/i2c/arducam-pivariety.h
  18
  19 --- a/drivers/media/i2c/Kconfig
  20 +++ b/drivers/media/i2c/Kconfig
  21 @@ -730,6 +730,17 @@ config VIDEO_APTINA_PLL
  22  config VIDEO_CCS_PLL
  23         tristate
  24
  25 +config VIDEO_ARDUCAM_PIVARIETY
  26 +       tristate "Arducam Pivariety sensor support"
  27 +       depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
  28 +       depends on MEDIA_CAMERA_SUPPORT
  29 +       help
  30 +         This is a Video4Linux2 sensor driver for the Arducam
  31 +         Pivariety camera series.
  32 +
  33 +         To compile this driver as a module, choose M here: the
  34 +         module will be called arducam-pivariety.
  35 +
  36  config VIDEO_HI556
  37         tristate "Hynix Hi-556 sensor support"
  38         depends on I2C && VIDEO_V4L2
  39 --- a/drivers/media/i2c/Makefile
  40 +++ b/drivers/media/i2c/Makefile
  41 @@ -7,6 +7,7 @@ obj-$(CONFIG_VIDEO_ET8EK8)      += et8ek8/
  42  obj-$(CONFIG_VIDEO_CX25840) += cx25840/
  43  obj-$(CONFIG_VIDEO_M5MOLS)     += m5mols/
  44
  45 +obj-$(CONFIG_VIDEO_ARDUCAM_PIVARIETY) += arducam-pivariety.o
  46  obj-$(CONFIG_VIDEO_APTINA_PLL) += aptina-pll.o
  47  obj-$(CONFIG_VIDEO_TVAUDIO) += tvaudio.o
  48  obj-$(CONFIG_VIDEO_TDA7432) += tda7432.o
  49 --- /dev/null
  50 +++ b/drivers/media/i2c/arducam-pivariety.c
  51 @@ -0,0 +1,1469 @@
  52 +// SPDX-License-Identifier: GPL-2.0
  53 +/*
  54 + * A V4L2 driver for Arducam Pivariety Cameras
  55 + * Copyright (C) 2022 Arducam Technology co., Ltd.
  56 + *
  57 + * Based on Sony IMX219 camera driver
  58 + * Copyright (C) 2019, Raspberry Pi (Trading) Ltd
  59 + *
  60 + * I2C read and write method is taken from the OV9281 driver
  61 + * Copyright (C) 2017 Fuzhou Rockchip Electronics Co., Ltd.
  62 + */
  63 +
  64 +#include <linux/clk.h>
  65 +#include <linux/delay.h>
  66 +#include <linux/gpio/consumer.h>
  67 +#include <linux/i2c.h>
  68 +#include <linux/module.h>
  69 +#include <linux/pm_runtime.h>
  70 +#include <linux/regulator/consumer.h>
  71 +#include <media/v4l2-ctrls.h>
  72 +#include <media/v4l2-device.h>
  73 +#include <media/v4l2-event.h>
  74 +#include <media/v4l2-fwnode.h>
  75 +#include "arducam-pivariety.h"
  76 +
  77 +static int debug;
  78 +module_param(debug, int, 0644);
  79 +
  80 +/* regulator supplies */
  81 +static const char * const pivariety_supply_name[] = {
  82 +       /* Supplies can be enabled in any order */
  83 +       "VANA",  /* Analog (2.8V) supply */
  84 +       "VDIG",  /* Digital Core (1.8V) supply */
  85 +       "VDDL",  /* IF (1.2V) supply */
  86 +};
  87 +
  88 +/* The supported raw formats. */
  89 +static const u32 codes[] = {
  90 +       MEDIA_BUS_FMT_SBGGR8_1X8,
  91 +       MEDIA_BUS_FMT_SGBRG8_1X8,
  92 +       MEDIA_BUS_FMT_SGRBG8_1X8,
  93 +       MEDIA_BUS_FMT_SRGGB8_1X8,
  94 +       MEDIA_BUS_FMT_Y8_1X8,
  95 +
  96 +       MEDIA_BUS_FMT_SBGGR10_1X10,
  97 +       MEDIA_BUS_FMT_SGBRG10_1X10,
  98 +       MEDIA_BUS_FMT_SGRBG10_1X10,
  99 +       MEDIA_BUS_FMT_SRGGB10_1X10,
 100 +       MEDIA_BUS_FMT_Y10_1X10,
 101 +
 102 +       MEDIA_BUS_FMT_SBGGR12_1X12,
 103 +       MEDIA_BUS_FMT_SGBRG12_1X12,
 104 +       MEDIA_BUS_FMT_SGRBG12_1X12,
 105 +       MEDIA_BUS_FMT_SRGGB12_1X12,
 106 +       MEDIA_BUS_FMT_Y12_1X12,
 107 +};
 108 +
 109 +#define ARDUCAM_NUM_SUPPLIES ARRAY_SIZE(pivariety_supply_name)
 110 +
 111 +#define ARDUCAM_XCLR_MIN_DELAY_US      10000
 112 +#define ARDUCAM_XCLR_DELAY_RANGE_US    1000
 113 +
 114 +#define MAX_CTRLS 32
 115 +
 116 +struct pivariety {
 117 +       struct v4l2_subdev sd;
 118 +       struct media_pad pad;
 119 +
 120 +       struct v4l2_fwnode_bus_mipi_csi2 bus;
 121 +       struct clk *xclk;
 122 +       u32 xclk_freq;
 123 +
 124 +       struct gpio_desc *reset_gpio;
 125 +       struct regulator_bulk_data supplies[ARDUCAM_NUM_SUPPLIES];
 126 +
 127 +       struct arducam_format *supported_formats;
 128 +       int num_supported_formats;
 129 +       int current_format_idx;
 130 +       int current_resolution_idx;
 131 +       int lanes;
 132 +       int bayer_order_volatile;
 133 +       bool wait_until_free;
 134 +
 135 +       struct v4l2_ctrl_handler ctrl_handler;
 136 +       struct v4l2_ctrl *ctrls[MAX_CTRLS];
 137 +       /* V4L2 Controls */
 138 +       struct v4l2_ctrl *vflip;
 139 +       struct v4l2_ctrl *hflip;
 140 +
 141 +       struct v4l2_rect crop;
 142 +       /*
 143 +        * Mutex for serialized access:
 144 +        * Protect sensor module set pad format and start/stop streaming safely.
 145 +        */
 146 +       struct mutex mutex;
 147 +
 148 +       /* Streaming on/off */
 149 +       bool streaming;
 150 +};
 151 +
 152 +static inline struct pivariety *to_pivariety(struct v4l2_subdev *_sd)
 153 +{
 154 +       return container_of(_sd, struct pivariety, sd);
 155 +}
 156 +
 157 +/* Write registers up to 4 at a time */
 158 +static int pivariety_write_reg(struct i2c_client *client, u16 reg, u32 val)
 159 +{
 160 +       unsigned int len = sizeof(u32);
 161 +       u32 buf_i, val_i = 0;
 162 +       u8 buf[6];
 163 +       u8 *val_p;
 164 +       __be32 val_be;
 165 +
 166 +       buf[0] = reg >> 8;
 167 +       buf[1] = reg & 0xff;
 168 +
 169 +       val_be = cpu_to_be32(val);
 170 +       val_p = (u8 *)&val_be;
 171 +       buf_i = 2;
 172 +
 173 +       while (val_i < 4)
 174 +               buf[buf_i++] = val_p[val_i++];
 175 +
 176 +       if (i2c_master_send(client, buf, len + 2) != len + 2)
 177 +               return -EIO;
 178 +
 179 +       return 0;
 180 +}
 181 +
 182 +/* Read registers up to 4 at a time */
 183 +static int pivariety_read_reg(struct i2c_client *client, u16 reg, u32 *val)
 184 +{
 185 +       struct i2c_msg msgs[2];
 186 +       unsigned int len = sizeof(u32);
 187 +       u8 *data_be_p;
 188 +       __be32 data_be = 0;
 189 +       __be16 reg_addr_be = cpu_to_be16(reg);
 190 +       int ret;
 191 +
 192 +       data_be_p = (u8 *)&data_be;
 193 +       /* Write register address */
 194 +       msgs[0].addr = client->addr;
 195 +       msgs[0].flags = 0;
 196 +       msgs[0].len = 2;
 197 +       msgs[0].buf = (u8 *)&reg_addr_be;
 198 +
 199 +       /* Read data from register */
 200 +       msgs[1].addr = client->addr;
 201 +       msgs[1].flags = I2C_M_RD;
 202 +       msgs[1].len = len;
 203 +       msgs[1].buf = data_be_p;
 204 +
 205 +       ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
 206 +       if (ret != ARRAY_SIZE(msgs))
 207 +               return -EIO;
 208 +
 209 +       *val = be32_to_cpu(data_be);
 210 +
 211 +       return 0;
 212 +}
 213 +
 214 +static int
 215 +pivariety_read(struct pivariety *pivariety, u16 addr, u32 *value)
 216 +{
 217 +       struct v4l2_subdev *sd = &pivariety->sd;
 218 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
 219 +       int ret, count = 0;
 220 +
 221 +       while (count++ < I2C_READ_RETRY_COUNT) {
 222 +               ret = pivariety_read_reg(client, addr, value);
 223 +               if (!ret) {
 224 +                       v4l2_dbg(2, debug, sd, "%s: 0x%02x 0x%04x\n",
 225 +                                __func__, addr, *value);
 226 +                       return ret;
 227 +               }
 228 +       }
 229 +
 230 +       v4l2_err(sd, "%s: Reading register 0x%02x failed\n",
 231 +                __func__, addr);
 232 +
 233 +       return ret;
 234 +}
 235 +
 236 +static int pivariety_write(struct pivariety *pivariety, u16 addr, u32 value)
 237 +{
 238 +       struct v4l2_subdev *sd = &pivariety->sd;
 239 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
 240 +       int ret, count = 0;
 241 +
 242 +       while (count++ < I2C_WRITE_RETRY_COUNT) {
 243 +               ret = pivariety_write_reg(client, addr, value);
 244 +               if (!ret)
 245 +                       return ret;
 246 +       }
 247 +
 248 +       v4l2_err(sd, "%s: Write 0x%04x to register 0x%02x failed\n",
 249 +                __func__, value, addr);
 250 +
 251 +       return ret;
 252 +}
 253 +
 254 +static int wait_for_free(struct pivariety *pivariety, int interval)
 255 +{
 256 +       u32 value;
 257 +       u32 count = 0;
 258 +
 259 +       while (count++ < (1000 / interval)) {
 260 +               int ret = pivariety_read(pivariety, SYSTEM_IDLE_REG, &value);
 261 +
 262 +               if (!ret && !value)
 263 +                       break;
 264 +               msleep(interval);
 265 +       }
 266 +
 267 +       v4l2_dbg(2, debug, &pivariety->sd, "%s: End wait, Count: %d.\n",
 268 +                __func__, count);
 269 +
 270 +       return 0;
 271 +}
 272 +
 273 +static int is_raw(int pixformat)
 274 +{
 275 +       return pixformat >= 0x28 && pixformat <= 0x2D;
 276 +}
 277 +
 278 +static u32 bayer_to_mbus_code(int data_type, int bayer_order)
 279 +{
 280 +       const u32 depth8[] = {
 281 +               MEDIA_BUS_FMT_SBGGR8_1X8,
 282 +               MEDIA_BUS_FMT_SGBRG8_1X8,
 283 +               MEDIA_BUS_FMT_SGRBG8_1X8,
 284 +               MEDIA_BUS_FMT_SRGGB8_1X8,
 285 +               MEDIA_BUS_FMT_Y8_1X8,
 286 +       };
 287 +
 288 +       const u32 depth10[] = {
 289 +               MEDIA_BUS_FMT_SBGGR10_1X10,
 290 +               MEDIA_BUS_FMT_SGBRG10_1X10,
 291 +               MEDIA_BUS_FMT_SGRBG10_1X10,
 292 +               MEDIA_BUS_FMT_SRGGB10_1X10,
 293 +               MEDIA_BUS_FMT_Y10_1X10,
 294 +       };
 295 +
 296 +       const u32 depth12[] = {
 297 +               MEDIA_BUS_FMT_SBGGR12_1X12,
 298 +               MEDIA_BUS_FMT_SGBRG12_1X12,
 299 +               MEDIA_BUS_FMT_SGRBG12_1X12,
 300 +               MEDIA_BUS_FMT_SRGGB12_1X12,
 301 +               MEDIA_BUS_FMT_Y12_1X12,
 302 +       };
 303 +
 304 +       if (bayer_order < 0 || bayer_order > 4)
 305 +               return 0;
 306 +
 307 +       switch (data_type) {
 308 +       case IMAGE_DT_RAW8:
 309 +               return depth8[bayer_order];
 310 +       case IMAGE_DT_RAW10:
 311 +               return depth10[bayer_order];
 312 +       case IMAGE_DT_RAW12:
 313 +               return depth12[bayer_order];
 314 +       }
 315 +
 316 +       return 0;
 317 +}
 318 +
 319 +static u32 yuv422_to_mbus_code(int data_type, int order)
 320 +{
 321 +       const u32 depth8[] = {
 322 +               MEDIA_BUS_FMT_YUYV8_1X16,
 323 +               MEDIA_BUS_FMT_YVYU8_1X16,
 324 +               MEDIA_BUS_FMT_UYVY8_1X16,
 325 +               MEDIA_BUS_FMT_VYUY8_1X16,
 326 +       };
 327 +
 328 +       const u32 depth10[] = {
 329 +               MEDIA_BUS_FMT_YUYV10_1X20,
 330 +               MEDIA_BUS_FMT_YVYU10_1X20,
 331 +               MEDIA_BUS_FMT_UYVY10_1X20,
 332 +               MEDIA_BUS_FMT_VYUY10_1X20,
 333 +       };
 334 +
 335 +       if (order < 0 || order > 3)
 336 +               return 0;
 337 +
 338 +       switch (data_type) {
 339 +       case IMAGE_DT_YUV422_8:
 340 +               return depth8[order];
 341 +       case IMAGE_DT_YUV422_10:
 342 +               return depth10[order];
 343 +       }
 344 +
 345 +       return 0;
 346 +}
 347 +
 348 +static u32 data_type_to_mbus_code(int data_type, int bayer_order)
 349 +{
 350 +       if (is_raw(data_type))
 351 +               return bayer_to_mbus_code(data_type, bayer_order);
 352 +
 353 +       switch (data_type) {
 354 +       case IMAGE_DT_YUV422_8:
 355 +       case IMAGE_DT_YUV422_10:
 356 +               return yuv422_to_mbus_code(data_type, bayer_order);
 357 +       case IMAGE_DT_RGB565:
 358 +               return MEDIA_BUS_FMT_RGB565_2X8_LE;
 359 +       case IMAGE_DT_RGB888:
 360 +               return MEDIA_BUS_FMT_RGB888_1X24;
 361 +       }
 362 +
 363 +       return 0;
 364 +}
 365 +
 366 +/* Get bayer order based on flip setting. */
 367 +static u32 pivariety_get_format_code(struct pivariety *pivariety,
 368 +                                    struct arducam_format *format)
 369 +{
 370 +       unsigned int order, origin_order;
 371 +
 372 +       lockdep_assert_held(&pivariety->mutex);
 373 +
 374 +       /*
 375 +        * Only the bayer format needs to transform the format.
 376 +        */
 377 +       if (!is_raw(format->data_type) ||
 378 +           !pivariety->bayer_order_volatile ||
 379 +           format->bayer_order == BAYER_ORDER_GRAY)
 380 +               return data_type_to_mbus_code(format->data_type,
 381 +                                             format->bayer_order);
 382 +
 383 +       order = format->bayer_order;
 384 +
 385 +       origin_order = order;
 386 +
 387 +       order = (pivariety->hflip && pivariety->hflip->val ? order ^ 1 : order);
 388 +       order = (pivariety->vflip && pivariety->vflip->val ? order ^ 2 : order);
 389 +
 390 +       v4l2_dbg(1, debug, &pivariety->sd, "%s: before: %d, after: %d.\n",
 391 +                __func__, origin_order, order);
 392 +
 393 +       return data_type_to_mbus_code(format->data_type, order);
 394 +}
 395 +
 396 +/* Power/clock management functions */
 397 +static int pivariety_power_on(struct device *dev)
 398 +{
 399 +       struct i2c_client *client = to_i2c_client(dev);
 400 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
 401 +       struct pivariety *pivariety = to_pivariety(sd);
 402 +       int ret;
 403 +
 404 +       ret = regulator_bulk_enable(ARDUCAM_NUM_SUPPLIES,
 405 +                                   pivariety->supplies);
 406 +       if (ret) {
 407 +               dev_err(dev, "%s: failed to enable regulators\n",
 408 +                       __func__);
 409 +               return ret;
 410 +       }
 411 +
 412 +       ret = clk_prepare_enable(pivariety->xclk);
 413 +       if (ret) {
 414 +               dev_err(dev, "%s: failed to enable clock\n",
 415 +                       __func__);
 416 +               goto reg_off;
 417 +       }
 418 +
 419 +       gpiod_set_value_cansleep(pivariety->reset_gpio, 1);
 420 +       usleep_range(ARDUCAM_XCLR_MIN_DELAY_US,
 421 +                    ARDUCAM_XCLR_MIN_DELAY_US + ARDUCAM_XCLR_DELAY_RANGE_US);
 422 +
 423 +       return 0;
 424 +
 425 +reg_off:
 426 +       regulator_bulk_disable(ARDUCAM_NUM_SUPPLIES, pivariety->supplies);
 427 +
 428 +       return ret;
 429 +}
 430 +
 431 +static int pivariety_power_off(struct device *dev)
 432 +{
 433 +       struct i2c_client *client = to_i2c_client(dev);
 434 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
 435 +       struct pivariety *pivariety = to_pivariety(sd);
 436 +
 437 +       gpiod_set_value_cansleep(pivariety->reset_gpio, 0);
 438 +       regulator_bulk_disable(ARDUCAM_NUM_SUPPLIES, pivariety->supplies);
 439 +       clk_disable_unprepare(pivariety->xclk);
 440 +
 441 +       return 0;
 442 +}
 443 +
 444 +static int pivariety_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
 445 +{
 446 +       struct pivariety *pivariety = to_pivariety(sd);
 447 +       struct v4l2_mbus_framefmt *try_fmt =
 448 +               v4l2_subdev_get_try_format(sd, fh->state, 0);
 449 +       struct arducam_format *def_fmt = &pivariety->supported_formats[0];
 450 +
 451 +       /* Initialize try_fmt */
 452 +       try_fmt->width = def_fmt->resolution_set->width;
 453 +       try_fmt->height = def_fmt->resolution_set->height;
 454 +       try_fmt->code = def_fmt->mbus_code;
 455 +       try_fmt->field = V4L2_FIELD_NONE;
 456 +
 457 +       return 0;
 458 +}
 459 +
 460 +static int pivariety_s_ctrl(struct v4l2_ctrl *ctrl)
 461 +{
 462 +       int ret, i;
 463 +       struct pivariety *pivariety =
 464 +               container_of(ctrl->handler, struct pivariety,
 465 +                            ctrl_handler);
 466 +       struct arducam_format *supported_fmts = pivariety->supported_formats;
 467 +       int num_supported_formats = pivariety->num_supported_formats;
 468 +
 469 +       v4l2_dbg(3, debug, &pivariety->sd, "%s: cid = (0x%X), value = (%d).\n",
 470 +                __func__, ctrl->id, ctrl->val);
 471 +
 472 +       ret = pivariety_write(pivariety, CTRL_ID_REG, ctrl->id);
 473 +       ret += pivariety_write(pivariety, CTRL_VALUE_REG, ctrl->val);
 474 +       if (ret < 0)
 475 +               return -EINVAL;
 476 +
 477 +       /* When flip is set, modify all bayer formats */
 478 +       if (ctrl->id == V4L2_CID_VFLIP || ctrl->id == V4L2_CID_HFLIP) {
 479 +               for (i = 0; i < num_supported_formats; i++) {
 480 +                       supported_fmts[i].mbus_code =
 481 +                               pivariety_get_format_code(pivariety,
 482 +                                                         &supported_fmts[i]);
 483 +               }
 484 +       }
 485 +
 486 +       /*
 487 +        * When starting streaming, controls are set in batches,
 488 +        * and the short interval will cause some controls to be unsuccessfully
 489 +        * set.
 490 +        */
 491 +       if (pivariety->wait_until_free)
 492 +               wait_for_free(pivariety, 1);
 493 +       else
 494 +               usleep_range(200, 210);
 495 +
 496 +       return 0;
 497 +}
 498 +
 499 +static const struct v4l2_ctrl_ops pivariety_ctrl_ops = {
 500 +       .s_ctrl = pivariety_s_ctrl,
 501 +};
 502 +
 503 +static int pivariety_enum_mbus_code(struct v4l2_subdev *sd,
 504 +                                   struct v4l2_subdev_state *sd_state,
 505 +                                   struct v4l2_subdev_mbus_code_enum *code)
 506 +{
 507 +       struct pivariety *pivariety = to_pivariety(sd);
 508 +       struct arducam_format *supported_formats = pivariety->supported_formats;
 509 +       int num_supported_formats = pivariety->num_supported_formats;
 510 +
 511 +       v4l2_dbg(1, debug, sd, "%s: index = (%d)\n", __func__, code->index);
 512 +
 513 +       if (code->index >= num_supported_formats)
 514 +               return -EINVAL;
 515 +
 516 +       code->code = supported_formats[code->index].mbus_code;
 517 +
 518 +       return 0;
 519 +}
 520 +
 521 +static int pivariety_enum_framesizes(struct v4l2_subdev *sd,
 522 +                                    struct v4l2_subdev_state *sd_state,
 523 +                                    struct v4l2_subdev_frame_size_enum *fse)
 524 +{
 525 +       int i;
 526 +       struct pivariety *pivariety = to_pivariety(sd);
 527 +       struct arducam_format *supported_formats = pivariety->supported_formats;
 528 +       int num_supported_formats = pivariety->num_supported_formats;
 529 +       struct arducam_format *format;
 530 +       struct arducam_resolution *resolution;
 531 +
 532 +       v4l2_dbg(1, debug, sd, "%s: code = (0x%X), index = (%d)\n",
 533 +                __func__, fse->code, fse->index);
 534 +
 535 +       for (i = 0; i < num_supported_formats; i++) {
 536 +               format = &supported_formats[i];
 537 +               if (fse->code == format->mbus_code) {
 538 +                       if (fse->index >= format->num_resolution_set)
 539 +                               return -EINVAL;
 540 +
 541 +                       resolution = &format->resolution_set[fse->index];
 542 +                       fse->min_width = resolution->width;
 543 +                       fse->max_width = resolution->width;
 544 +                       fse->min_height = resolution->height;
 545 +                       fse->max_height = resolution->height;
 546 +
 547 +                       return 0;
 548 +               }
 549 +       }
 550 +
 551 +       return -EINVAL;
 552 +}
 553 +
 554 +static int pivariety_get_fmt(struct v4l2_subdev *sd,
 555 +                            struct v4l2_subdev_state *sd_state,
 556 +                            struct v4l2_subdev_format *format)
 557 +{
 558 +       struct pivariety *pivariety = to_pivariety(sd);
 559 +       struct arducam_format *current_format;
 560 +       struct v4l2_mbus_framefmt *fmt = &format->format;
 561 +       int cur_res_idx;
 562 +
 563 +       if (format->pad != 0)
 564 +               return -EINVAL;
 565 +
 566 +       mutex_lock(&pivariety->mutex);
 567 +
 568 +       current_format =
 569 +               &pivariety->supported_formats[pivariety->current_format_idx];
 570 +       cur_res_idx = pivariety->current_resolution_idx;
 571 +       format->format.width =
 572 +               current_format->resolution_set[cur_res_idx].width;
 573 +       format->format.height =
 574 +               current_format->resolution_set[cur_res_idx].height;
 575 +       format->format.code = current_format->mbus_code;
 576 +       format->format.field = V4L2_FIELD_NONE;
 577 +       fmt->colorspace = V4L2_COLORSPACE_RAW;
 578 +       fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(fmt->colorspace);
 579 +       fmt->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
 580 +                                                         fmt->colorspace,
 581 +                                                         fmt->ycbcr_enc);
 582 +       fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(fmt->colorspace);
 583 +
 584 +       v4l2_dbg(1, debug, sd, "%s: width: (%d) height: (%d) code: (0x%X)\n",
 585 +                __func__, format->format.width, format->format.height,
 586 +                format->format.code);
 587 +
 588 +       mutex_unlock(&pivariety->mutex);
 589 +       return 0;
 590 +}
 591 +
 592 +static int pivariety_get_fmt_idx_by_code(struct pivariety *pivariety,
 593 +                                        u32 mbus_code)
 594 +{
 595 +       int i;
 596 +       u32 data_type;
 597 +       struct arducam_format *formats = pivariety->supported_formats;
 598 +
 599 +       for (i = 0; i < pivariety->num_supported_formats; i++) {
 600 +               if (formats[i].mbus_code == mbus_code)
 601 +                       return i;
 602 +       }
 603 +
 604 +       /*
 605 +        * If the specified format is not found in the list of supported
 606 +        * formats, try to find a format of the same data type.
 607 +        */
 608 +       for (i = 0; i < ARRAY_SIZE(codes); i++)
 609 +               if (codes[i] == mbus_code)
 610 +                       break;
 611 +
 612 +       if (i >= ARRAY_SIZE(codes))
 613 +               return -EINVAL;
 614 +
 615 +       data_type = i / 5 + IMAGE_DT_RAW8;
 616 +
 617 +       for (i = 0; i < pivariety->num_supported_formats; i++) {
 618 +               if (formats[i].data_type == data_type)
 619 +                       return i;
 620 +       }
 621 +
 622 +       return -EINVAL;
 623 +}
 624 +
 625 +static struct v4l2_ctrl *get_control(struct pivariety *pivariety,
 626 +                                    u32 id)
 627 +{
 628 +       int index = 0;
 629 +
 630 +       while (index < MAX_CTRLS && pivariety->ctrls[index]) {
 631 +               if (pivariety->ctrls[index]->id == id)
 632 +                       return pivariety->ctrls[index];
 633 +               index++;
 634 +       }
 635 +
 636 +       return NULL;
 637 +}
 638 +
 639 +static int update_control(struct pivariety *pivariety, u32 id)
 640 +{
 641 +       struct v4l2_subdev *sd = &pivariety->sd;
 642 +       struct v4l2_ctrl *ctrl;
 643 +       u32 min, max, step, def, id2;
 644 +       int ret = 0;
 645 +
 646 +       pivariety_write(pivariety, CTRL_ID_REG, id);
 647 +       pivariety_read(pivariety, CTRL_ID_REG, &id2);
 648 +
 649 +       v4l2_dbg(1, debug, sd, "%s: Write ID: 0x%08X Read ID: 0x%08X\n",
 650 +                __func__, id, id2);
 651 +
 652 +       pivariety_write(pivariety, CTRL_VALUE_REG, 0);
 653 +       wait_for_free(pivariety, 1);
 654 +
 655 +       ret += pivariety_read(pivariety, CTRL_MAX_REG, &max);
 656 +       ret += pivariety_read(pivariety, CTRL_MIN_REG, &min);
 657 +       ret += pivariety_read(pivariety, CTRL_DEF_REG, &def);
 658 +       ret += pivariety_read(pivariety, CTRL_STEP_REG, &step);
 659 +
 660 +       if (ret < 0)
 661 +               goto err;
 662 +
 663 +       if (id == NO_DATA_AVAILABLE || max == NO_DATA_AVAILABLE ||
 664 +           min == NO_DATA_AVAILABLE || def == NO_DATA_AVAILABLE ||
 665 +           step == NO_DATA_AVAILABLE)
 666 +               goto err;
 667 +
 668 +       v4l2_dbg(1, debug, sd, "%s: min: %d, max: %d, step: %d, def: %d\n",
 669 +                __func__, min, max, step, def);
 670 +
 671 +       ctrl = get_control(pivariety, id);
 672 +       return __v4l2_ctrl_modify_range(ctrl, min, max, step, def);
 673 +
 674 +err:
 675 +       return -EINVAL;
 676 +}
 677 +
 678 +static int update_controls(struct pivariety *pivariety)
 679 +{
 680 +       int ret = 0;
 681 +       int index = 0;
 682 +
 683 +       wait_for_free(pivariety, 5);
 684 +
 685 +       while (index < MAX_CTRLS && pivariety->ctrls[index]) {
 686 +               ret += update_control(pivariety, pivariety->ctrls[index]->id);
 687 +               index++;
 688 +       }
 689 +
 690 +       return ret;
 691 +}
 692 +
 693 +static int pivariety_set_fmt(struct v4l2_subdev *sd,
 694 +                            struct v4l2_subdev_state *sd_state,
 695 +                            struct v4l2_subdev_format *format)
 696 +{
 697 +       int i, j;
 698 +       struct pivariety *pivariety = to_pivariety(sd);
 699 +       struct arducam_format *supported_formats = pivariety->supported_formats;
 700 +
 701 +       if (format->pad != 0)
 702 +               return -EINVAL;
 703 +
 704 +       mutex_lock(&pivariety->mutex);
 705 +
 706 +       format->format.colorspace = V4L2_COLORSPACE_RAW;
 707 +       format->format.field = V4L2_FIELD_NONE;
 708 +
 709 +       v4l2_dbg(1, debug, sd, "%s: code: 0x%X, width: %d, height: %d\n",
 710 +                __func__, format->format.code, format->format.width,
 711 +                format->format.height);
 712 +
 713 +       i = pivariety_get_fmt_idx_by_code(pivariety, format->format.code);
 714 +       if (i < 0)
 715 +               i = 0;
 716 +
 717 +       format->format.code = supported_formats[i].mbus_code;
 718 +
 719 +       for (j = 0; j < supported_formats[i].num_resolution_set; j++) {
 720 +               if (supported_formats[i].resolution_set[j].width ==
 721 +                                               format->format.width &&
 722 +                       supported_formats[i].resolution_set[j].height ==
 723 +                                               format->format.height) {
 724 +                       v4l2_dbg(1, debug, sd,
 725 +                                "%s: format match.\n", __func__);
 726 +                       v4l2_dbg(1, debug, sd,
 727 +                                "%s: set format to device: %d %d.\n",
 728 +                                __func__, supported_formats[i].index, j);
 729 +
 730 +                       pivariety_write(pivariety, PIXFORMAT_INDEX_REG,
 731 +                                       supported_formats[i].index);
 732 +                       pivariety_write(pivariety, RESOLUTION_INDEX_REG, j);
 733 +
 734 +                       pivariety->current_format_idx = i;
 735 +                       pivariety->current_resolution_idx = j;
 736 +
 737 +                       update_controls(pivariety);
 738 +
 739 +                       goto unlock;
 740 +               }
 741 +       }
 742 +
 743 +       format->format.width = supported_formats[i].resolution_set[0].width;
 744 +       format->format.height = supported_formats[i].resolution_set[0].height;
 745 +
 746 +       pivariety_write(pivariety, PIXFORMAT_INDEX_REG,
 747 +                       supported_formats[i].index);
 748 +       pivariety_write(pivariety, RESOLUTION_INDEX_REG, 0);
 749 +
 750 +       pivariety->current_format_idx = i;
 751 +       pivariety->current_resolution_idx = 0;
 752 +       update_controls(pivariety);
 753 +
 754 +unlock:
 755 +
 756 +       mutex_unlock(&pivariety->mutex);
 757 +
 758 +       return 0;
 759 +}
 760 +
 761 +/* Start streaming */
 762 +static int pivariety_start_streaming(struct pivariety *pivariety)
 763 +{
 764 +       int ret;
 765 +
 766 +       /* set stream on register */
 767 +       ret = pivariety_write(pivariety, MODE_SELECT_REG,
 768 +                             ARDUCAM_MODE_STREAMING);
 769 +
 770 +       if (ret)
 771 +               return ret;
 772 +
 773 +       wait_for_free(pivariety, 2);
 774 +
 775 +       /*
 776 +        * When starting streaming, controls are set in batches,
 777 +        * and the short interval will cause some controls to be unsuccessfully
 778 +        * set.
 779 +        */
 780 +       pivariety->wait_until_free = true;
 781 +       /* Apply customized values from user */
 782 +       ret =  __v4l2_ctrl_handler_setup(pivariety->sd.ctrl_handler);
 783 +
 784 +       pivariety->wait_until_free = false;
 785 +       if (ret)
 786 +               return ret;
 787 +
 788 +       wait_for_free(pivariety, 2);
 789 +
 790 +       return ret;
 791 +}
 792 +
 793 +static int pivariety_read_sel(struct pivariety *pivariety,
 794 +                             struct v4l2_rect *rect)
 795 +{
 796 +       int ret = 0;
 797 +
 798 +       ret += pivariety_read(pivariety, IPC_SEL_TOP_REG, &rect->top);
 799 +       ret += pivariety_read(pivariety, IPC_SEL_LEFT_REG, &rect->left);
 800 +       ret += pivariety_read(pivariety, IPC_SEL_WIDTH_REG, &rect->width);
 801 +       ret += pivariety_read(pivariety, IPC_SEL_HEIGHT_REG, &rect->height);
 802 +
 803 +       if (ret || rect->top == NO_DATA_AVAILABLE ||
 804 +           rect->left == NO_DATA_AVAILABLE ||
 805 +           rect->width == NO_DATA_AVAILABLE ||
 806 +           rect->height == NO_DATA_AVAILABLE) {
 807 +               v4l2_err(&pivariety->sd, "%s: Failed to read selection.\n",
 808 +                        __func__);
 809 +               return -EINVAL;
 810 +               }
 811 +
 812 +       return 0;
 813 +}
 814 +
 815 +static const struct v4l2_rect *
 816 +__pivariety_get_pad_crop(struct pivariety *pivariety,
 817 +                        struct v4l2_subdev_state *sd_state,
 818 +                        unsigned int pad,
 819 +                        enum v4l2_subdev_format_whence which)
 820 +{
 821 +       int ret;
 822 +
 823 +       switch (which) {
 824 +       case V4L2_SUBDEV_FORMAT_TRY:
 825 +               return v4l2_subdev_get_try_crop(&pivariety->sd, sd_state, pad);
 826 +       case V4L2_SUBDEV_FORMAT_ACTIVE:
 827 +               ret = pivariety_read_sel(pivariety, &pivariety->crop);
 828 +               if (ret)
 829 +                       return NULL;
 830 +               return &pivariety->crop;
 831 +       }
 832 +
 833 +       return NULL;
 834 +}
 835 +
 836 +static int pivariety_get_selection(struct v4l2_subdev *sd,
 837 +                                  struct v4l2_subdev_state *sd_state,
 838 +                                  struct v4l2_subdev_selection *sel)
 839 +{
 840 +       int ret = 0;
 841 +       struct v4l2_rect rect;
 842 +       struct pivariety *pivariety = to_pivariety(sd);
 843 +
 844 +       ret = pivariety_write(pivariety, IPC_SEL_TARGET_REG, sel->target);
 845 +       if (ret) {
 846 +               v4l2_err(sd, "%s: Write register 0x%02x failed\n",
 847 +                        __func__, IPC_SEL_TARGET_REG);
 848 +               return -EINVAL;
 849 +       }
 850 +
 851 +       wait_for_free(pivariety, 2);
 852 +
 853 +       switch (sel->target) {
 854 +       case V4L2_SEL_TGT_CROP: {
 855 +               mutex_lock(&pivariety->mutex);
 856 +               sel->r = *__pivariety_get_pad_crop(pivariety, sd_state,
 857 +                                                  sel->pad,
 858 +                                                  sel->which);
 859 +               mutex_unlock(&pivariety->mutex);
 860 +
 861 +               return 0;
 862 +       }
 863 +
 864 +       case V4L2_SEL_TGT_NATIVE_SIZE:
 865 +       case V4L2_SEL_TGT_CROP_DEFAULT:
 866 +       case V4L2_SEL_TGT_CROP_BOUNDS:
 867 +               ret = pivariety_read_sel(pivariety, &rect);
 868 +               if (ret)
 869 +                       return -EINVAL;
 870 +
 871 +               sel->r = rect;
 872 +               return 0;
 873 +       }
 874 +
 875 +       return -EINVAL;
 876 +}
 877 +
 878 +/* Stop streaming */
 879 +static int pivariety_stop_streaming(struct pivariety *pivariety)
 880 +{
 881 +       int ret;
 882 +
 883 +       /* set stream off register */
 884 +       ret = pivariety_write(pivariety, MODE_SELECT_REG, ARDUCAM_MODE_STANDBY);
 885 +       if (ret)
 886 +               v4l2_err(&pivariety->sd, "%s failed to set stream\n", __func__);
 887 +
 888 +       /*
 889 +        * Return success even if it was an error, as there is nothing the
 890 +        * caller can do about it.
 891 +        */
 892 +       return 0;
 893 +}
 894 +
 895 +static int pivariety_set_stream(struct v4l2_subdev *sd, int enable)
 896 +{
 897 +       struct pivariety *pivariety = to_pivariety(sd);
 898 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
 899 +       int ret = 0;
 900 +
 901 +       mutex_lock(&pivariety->mutex);
 902 +       if (pivariety->streaming == enable) {
 903 +               mutex_unlock(&pivariety->mutex);
 904 +               return 0;
 905 +       }
 906 +
 907 +       if (enable) {
 908 +               ret = pm_runtime_get_sync(&client->dev);
 909 +               if (ret < 0) {
 910 +                       pm_runtime_put_noidle(&client->dev);
 911 +                       goto err_unlock;
 912 +               }
 913 +
 914 +               /*
 915 +                * Apply default & customized values
 916 +                * and then start streaming.
 917 +                */
 918 +               ret = pivariety_start_streaming(pivariety);
 919 +               if (ret)
 920 +                       goto err_rpm_put;
 921 +       } else {
 922 +               pivariety_stop_streaming(pivariety);
 923 +               pm_runtime_put(&client->dev);
 924 +       }
 925 +
 926 +       pivariety->streaming = enable;
 927 +
 928 +       /*
 929 +        * vflip and hflip cannot change during streaming
 930 +        * Pivariety may not implement flip control.
 931 +        */
 932 +       if (pivariety->vflip)
 933 +               __v4l2_ctrl_grab(pivariety->vflip, enable);
 934 +
 935 +       if (pivariety->hflip)
 936 +               __v4l2_ctrl_grab(pivariety->hflip, enable);
 937 +
 938 +       mutex_unlock(&pivariety->mutex);
 939 +
 940 +       return ret;
 941 +
 942 +err_rpm_put:
 943 +       pm_runtime_put(&client->dev);
 944 +err_unlock:
 945 +       mutex_unlock(&pivariety->mutex);
 946 +
 947 +       return ret;
 948 +}
 949 +
 950 +static int __maybe_unused pivariety_suspend(struct device *dev)
 951 +{
 952 +       struct i2c_client *client = to_i2c_client(dev);
 953 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
 954 +       struct pivariety *pivariety = to_pivariety(sd);
 955 +
 956 +       if (pivariety->streaming)
 957 +               pivariety_stop_streaming(pivariety);
 958 +
 959 +       return 0;
 960 +}
 961 +
 962 +static int __maybe_unused pivariety_resume(struct device *dev)
 963 +{
 964 +       struct i2c_client *client = to_i2c_client(dev);
 965 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
 966 +       struct pivariety *pivariety = to_pivariety(sd);
 967 +       int ret;
 968 +
 969 +       if (pivariety->streaming) {
 970 +               ret = pivariety_start_streaming(pivariety);
 971 +               if (ret)
 972 +                       goto error;
 973 +       }
 974 +
 975 +       return 0;
 976 +
 977 +error:
 978 +       pivariety_stop_streaming(pivariety);
 979 +       pivariety->streaming = 0;
 980 +       return ret;
 981 +}
 982 +
 983 +static int pivariety_get_regulators(struct pivariety *pivariety)
 984 +{
 985 +       struct i2c_client *client = v4l2_get_subdevdata(&pivariety->sd);
 986 +       int i;
 987 +
 988 +       for (i = 0; i < ARDUCAM_NUM_SUPPLIES; i++)
 989 +               pivariety->supplies[i].supply = pivariety_supply_name[i];
 990 +
 991 +       return devm_regulator_bulk_get(&client->dev,
 992 +                                      ARDUCAM_NUM_SUPPLIES,
 993 +                                      pivariety->supplies);
 994 +}
 995 +
 996 +static int pivariety_get_mbus_config(struct v4l2_subdev *sd, unsigned int pad,
 997 +                                    struct v4l2_mbus_config *cfg)
 998 +{
 999 +       struct pivariety *pivariety = to_pivariety(sd);
1000 +       const u32 mask = V4L2_MBUS_CSI2_LANE_MASK;
1001 +
1002 +       if (pivariety->lanes > pivariety->bus.num_data_lanes)
1003 +               return -EINVAL;
1004 +
1005 +       cfg->type = V4L2_MBUS_CSI2_DPHY;
1006 +       cfg->flags = (pivariety->lanes << __ffs(mask)) & mask;
1007 +
1008 +       return 0;
1009 +}
1010 +
1011 +static const struct v4l2_subdev_core_ops pivariety_core_ops = {
1012 +       .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
1013 +       .unsubscribe_event = v4l2_event_subdev_unsubscribe,
1014 +};
1015 +
1016 +static const struct v4l2_subdev_video_ops pivariety_video_ops = {
1017 +       .s_stream = pivariety_set_stream,
1018 +};
1019 +
1020 +static const struct v4l2_subdev_pad_ops pivariety_pad_ops = {
1021 +       .enum_mbus_code = pivariety_enum_mbus_code,
1022 +       .get_fmt = pivariety_get_fmt,
1023 +       .set_fmt = pivariety_set_fmt,
1024 +       .enum_frame_size = pivariety_enum_framesizes,
1025 +       .get_selection = pivariety_get_selection,
1026 +       .get_mbus_config = pivariety_get_mbus_config,
1027 +};
1028 +
1029 +static const struct v4l2_subdev_ops pivariety_subdev_ops = {
1030 +       .core = &pivariety_core_ops,
1031 +       .video = &pivariety_video_ops,
1032 +       .pad = &pivariety_pad_ops,
1033 +};
1034 +
1035 +static const struct v4l2_subdev_internal_ops pivariety_internal_ops = {
1036 +       .open = pivariety_open,
1037 +};
1038 +
1039 +static void pivariety_free_controls(struct pivariety *pivariety)
1040 +{
1041 +       v4l2_ctrl_handler_free(pivariety->sd.ctrl_handler);
1042 +       mutex_destroy(&pivariety->mutex);
1043 +}
1044 +
1045 +static int pivariety_get_length_of_set(struct pivariety *pivariety,
1046 +                                      u16 idx_reg, u16 val_reg)
1047 +{
1048 +       int ret;
1049 +       int index = 0;
1050 +       u32 val;
1051 +
1052 +       while (1) {
1053 +               ret = pivariety_write(pivariety, idx_reg, index);
1054 +               ret += pivariety_read(pivariety, val_reg, &val);
1055 +
1056 +               if (ret < 0)
1057 +                       return -1;
1058 +
1059 +               if (val == NO_DATA_AVAILABLE)
1060 +                       break;
1061 +               index++;
1062 +       }
1063 +       pivariety_write(pivariety, idx_reg, 0);
1064 +       return index;
1065 +}
1066 +
1067 +static int pivariety_enum_resolution(struct pivariety *pivariety,
1068 +                                    struct arducam_format *format)
1069 +{
1070 +       struct i2c_client *client = v4l2_get_subdevdata(&pivariety->sd);
1071 +       int index = 0;
1072 +       u32 width, height;
1073 +       int num_resolution = 0;
1074 +       int ret;
1075 +
1076 +       num_resolution = pivariety_get_length_of_set(pivariety,
1077 +                                                    RESOLUTION_INDEX_REG,
1078 +                                                    FORMAT_WIDTH_REG);
1079 +       if (num_resolution < 0)
1080 +               goto err;
1081 +
1082 +       format->resolution_set = devm_kzalloc(&client->dev,
1083 +                                             sizeof(*format->resolution_set) *
1084 +                                                               num_resolution,
1085 +                                             GFP_KERNEL);
1086 +       while (1) {
1087 +               ret = pivariety_write(pivariety, RESOLUTION_INDEX_REG, index);
1088 +               ret += pivariety_read(pivariety, FORMAT_WIDTH_REG, &width);
1089 +               ret += pivariety_read(pivariety, FORMAT_HEIGHT_REG, &height);
1090 +
1091 +               if (ret < 0)
1092 +                       goto err;
1093 +
1094 +               if (width == NO_DATA_AVAILABLE || height == NO_DATA_AVAILABLE)
1095 +                       break;
1096 +
1097 +               format->resolution_set[index].width = width;
1098 +               format->resolution_set[index].height = height;
1099 +
1100 +               index++;
1101 +       }
1102 +
1103 +       format->num_resolution_set = index;
1104 +       pivariety_write(pivariety, RESOLUTION_INDEX_REG, 0);
1105 +       return 0;
1106 +err:
1107 +       return -ENODEV;
1108 +}
1109 +
1110 +static int pivariety_enum_pixformat(struct pivariety *pivariety)
1111 +{
1112 +       int ret = 0;
1113 +       u32 mbus_code = 0;
1114 +       int pixfmt_type;
1115 +       int bayer_order;
1116 +       int bayer_order_not_volatile;
1117 +       int lanes;
1118 +       int index = 0;
1119 +       int num_pixformat = 0;
1120 +       struct arducam_format *arducam_fmt;
1121 +       struct i2c_client *client = v4l2_get_subdevdata(&pivariety->sd);
1122 +
1123 +       num_pixformat = pivariety_get_length_of_set(pivariety,
1124 +                                                   PIXFORMAT_INDEX_REG,
1125 +                                                   PIXFORMAT_TYPE_REG);
1126 +
1127 +       if (num_pixformat < 0)
1128 +               goto err;
1129 +
1130 +       ret = pivariety_read(pivariety, FLIPS_DONT_CHANGE_ORDER_REG,
1131 +                            &bayer_order_not_volatile);
1132 +       if (bayer_order_not_volatile == NO_DATA_AVAILABLE)
1133 +               pivariety->bayer_order_volatile = 1;
1134 +       else
1135 +               pivariety->bayer_order_volatile = !bayer_order_not_volatile;
1136 +
1137 +       if (ret < 0)
1138 +               goto err;
1139 +
1140 +       pivariety->supported_formats =
1141 +               devm_kzalloc(&client->dev,
1142 +                            sizeof(*pivariety->supported_formats) *
1143 +                                                               num_pixformat,
1144 +                            GFP_KERNEL);
1145 +
1146 +       while (1) {
1147 +               ret = pivariety_write(pivariety, PIXFORMAT_INDEX_REG, index);
1148 +               ret += pivariety_read(pivariety, PIXFORMAT_TYPE_REG,
1149 +                                     &pixfmt_type);
1150 +
1151 +               if (pixfmt_type == NO_DATA_AVAILABLE)
1152 +                       break;
1153 +
1154 +               ret += pivariety_read(pivariety, MIPI_LANES_REG, &lanes);
1155 +               if (lanes == NO_DATA_AVAILABLE)
1156 +                       break;
1157 +
1158 +               ret += pivariety_read(pivariety, PIXFORMAT_ORDER_REG,
1159 +                                     &bayer_order);
1160 +               if (ret < 0)
1161 +                       goto err;
1162 +
1163 +               mbus_code = data_type_to_mbus_code(pixfmt_type, bayer_order);
1164 +               arducam_fmt = &pivariety->supported_formats[index];
1165 +               arducam_fmt->index = index;
1166 +               arducam_fmt->mbus_code = mbus_code;
1167 +               arducam_fmt->bayer_order = bayer_order;
1168 +               arducam_fmt->data_type = pixfmt_type;
1169 +               if (pivariety_enum_resolution(pivariety, arducam_fmt))
1170 +                       goto err;
1171 +
1172 +               index++;
1173 +       }
1174 +
1175 +       pivariety_write(pivariety, PIXFORMAT_INDEX_REG, 0);
1176 +       pivariety->num_supported_formats = index;
1177 +       pivariety->current_format_idx = 0;
1178 +       pivariety->current_resolution_idx = 0;
1179 +       pivariety->lanes = lanes;
1180 +
1181 +       return 0;
1182 +
1183 +err:
1184 +       return -ENODEV;
1185 +}
1186 +
1187 +static const char *pivariety_ctrl_get_name(u32 id)
1188 +{
1189 +       switch (id) {
1190 +       case V4L2_CID_ARDUCAM_EXT_TRI:
1191 +               return "trigger_mode";
1192 +       case V4L2_CID_ARDUCAM_IRCUT:
1193 +               return "ircut";
1194 +       default:
1195 +               return NULL;
1196 +       }
1197 +}
1198 +
1199 +enum v4l2_ctrl_type pivariety_get_v4l2_ctrl_type(u32 id)
1200 +{
1201 +       switch (id) {
1202 +       case V4L2_CID_ARDUCAM_EXT_TRI:
1203 +               return V4L2_CTRL_TYPE_BOOLEAN;
1204 +       case V4L2_CID_ARDUCAM_IRCUT:
1205 +               return V4L2_CTRL_TYPE_BOOLEAN;
1206 +       default:
1207 +               return V4L2_CTRL_TYPE_INTEGER;
1208 +       }
1209 +}
1210 +
1211 +static struct v4l2_ctrl *v4l2_ctrl_new_arducam(struct v4l2_ctrl_handler *hdl,
1212 +                                              const struct v4l2_ctrl_ops *ops,
1213 +                                              u32 id, s64 min, s64 max,
1214 +                                              u64 step, s64 def)
1215 +{
1216 +       struct v4l2_ctrl_config ctrl_cfg = {
1217 +               .ops = ops,
1218 +               .id = id,
1219 +               .name = NULL,
1220 +               .type = V4L2_CTRL_TYPE_INTEGER,
1221 +               .flags = 0,
1222 +               .min = min,
1223 +               .max = max,
1224 +               .def = def,
1225 +               .step = step,
1226 +       };
1227 +
1228 +       ctrl_cfg.name = pivariety_ctrl_get_name(id);
1229 +       ctrl_cfg.type = pivariety_get_v4l2_ctrl_type(id);
1230 +
1231 +       return v4l2_ctrl_new_custom(hdl, &ctrl_cfg, NULL);
1232 +}
1233 +
1234 +static int pivariety_enum_controls(struct pivariety *pivariety)
1235 +{
1236 +       struct v4l2_subdev *sd = &pivariety->sd;
1237 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
1238 +       struct v4l2_ctrl_handler *ctrl_hdlr = &pivariety->ctrl_handler;
1239 +       struct v4l2_fwnode_device_properties props;
1240 +       struct v4l2_ctrl **ctrl = pivariety->ctrls;
1241 +       int ret, index, num_ctrls;
1242 +       u32 id, min, max, def, step;
1243 +
1244 +       num_ctrls = pivariety_get_length_of_set(pivariety, CTRL_INDEX_REG,
1245 +                                               CTRL_ID_REG);
1246 +       if (num_ctrls < 0)
1247 +               goto err;
1248 +
1249 +       v4l2_dbg(1, debug, sd, "%s: num_ctrls = %d\n",
1250 +                __func__, num_ctrls);
1251 +
1252 +       ret = v4l2_ctrl_handler_init(ctrl_hdlr, num_ctrls);
1253 +       if (ret)
1254 +               return ret;
1255 +
1256 +       index = 0;
1257 +       while (1) {
1258 +               ret = pivariety_write(pivariety, CTRL_INDEX_REG, index);
1259 +               pivariety_write(pivariety, CTRL_VALUE_REG, 0);
1260 +               wait_for_free(pivariety, 1);
1261 +
1262 +               ret += pivariety_read(pivariety, CTRL_ID_REG, &id);
1263 +               ret += pivariety_read(pivariety, CTRL_MAX_REG, &max);
1264 +               ret += pivariety_read(pivariety, CTRL_MIN_REG, &min);
1265 +               ret += pivariety_read(pivariety, CTRL_DEF_REG, &def);
1266 +               ret += pivariety_read(pivariety, CTRL_STEP_REG, &step);
1267 +               if (ret < 0)
1268 +                       goto err;
1269 +
1270 +               if (id == NO_DATA_AVAILABLE || max == NO_DATA_AVAILABLE ||
1271 +                   min == NO_DATA_AVAILABLE || def == NO_DATA_AVAILABLE ||
1272 +                   step == NO_DATA_AVAILABLE)
1273 +                       break;
1274 +
1275 +               v4l2_dbg(1, debug, sd,
1276 +                        "%s: index = %d, id = 0x%x, max = %d, min = %d, def = %d, step = %d\n",
1277 +                        __func__, index, id, max, min, def, step);
1278 +
1279 +               if (v4l2_ctrl_get_name(id)) {
1280 +                       *ctrl = v4l2_ctrl_new_std(ctrl_hdlr,
1281 +                                                 &pivariety_ctrl_ops,
1282 +                                                 id, min,
1283 +                                                 max, step,
1284 +                                                 def);
1285 +                       v4l2_dbg(1, debug, sd, "%s: ctrl: 0x%p\n",
1286 +                                __func__, *ctrl);
1287 +               } else if (pivariety_ctrl_get_name(id)) {
1288 +                       *ctrl = v4l2_ctrl_new_arducam(ctrl_hdlr,
1289 +                                                     &pivariety_ctrl_ops,
1290 +                                                     id, min, max, step, def);
1291 +
1292 +                       v4l2_dbg(1, debug, sd,
1293 +                                "%s: new custom ctrl, ctrl: 0x%p.\n",
1294 +                                __func__, *ctrl);
1295 +               } else {
1296 +                       index++;
1297 +                       continue;
1298 +               }
1299 +
1300 +               if (!*ctrl)
1301 +                       goto err;
1302 +
1303 +               switch (id) {
1304 +               case V4L2_CID_HFLIP:
1305 +                       pivariety->hflip = *ctrl;
1306 +                       if (pivariety->bayer_order_volatile)
1307 +                               pivariety->hflip->flags |=
1308 +                                               V4L2_CTRL_FLAG_MODIFY_LAYOUT;
1309 +                       break;
1310 +
1311 +               case V4L2_CID_VFLIP:
1312 +                       pivariety->vflip = *ctrl;
1313 +                       if (pivariety->bayer_order_volatile)
1314 +                               pivariety->vflip->flags |=
1315 +                                               V4L2_CTRL_FLAG_MODIFY_LAYOUT;
1316 +                       break;
1317 +
1318 +               case V4L2_CID_HBLANK:
1319 +                       (*ctrl)->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1320 +                       break;
1321 +               }
1322 +
1323 +               ctrl++;
1324 +               index++;
1325 +       }
1326 +
1327 +       pivariety_write(pivariety, CTRL_INDEX_REG, 0);
1328 +
1329 +       ret = v4l2_fwnode_device_parse(&client->dev, &props);
1330 +       if (ret)
1331 +               goto err;
1332 +
1333 +       ret = v4l2_ctrl_new_fwnode_properties(ctrl_hdlr,
1334 +                                             &pivariety_ctrl_ops,
1335 +                                             &props);
1336 +       if (ret)
1337 +               goto err;
1338 +
1339 +       pivariety->sd.ctrl_handler = ctrl_hdlr;
1340 +       v4l2_ctrl_handler_setup(ctrl_hdlr);
1341 +       return 0;
1342 +err:
1343 +       return -ENODEV;
1344 +}
1345 +
1346 +static int pivariety_parse_dt(struct pivariety *pivariety, struct device *dev)
1347 +{
1348 +       struct fwnode_handle *endpoint;
1349 +       struct v4l2_fwnode_endpoint ep_cfg = {
1350 +               .bus_type = V4L2_MBUS_CSI2_DPHY
1351 +       };
1352 +       int ret = -EINVAL;
1353 +
1354 +       /* Get CSI2 bus config */
1355 +       endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(dev), NULL);
1356 +       if (!endpoint) {
1357 +               dev_err(dev, "endpoint node not found\n");
1358 +               return -EINVAL;
1359 +       }
1360 +
1361 +       if (v4l2_fwnode_endpoint_alloc_parse(endpoint, &ep_cfg)) {
1362 +               dev_err(dev, "could not parse endpoint\n");
1363 +               goto error_out;
1364 +       }
1365 +
1366 +       pivariety->bus = ep_cfg.bus.mipi_csi2;
1367 +
1368 +       ret = 0;
1369 +
1370 +error_out:
1371 +       v4l2_fwnode_endpoint_free(&ep_cfg);
1372 +       fwnode_handle_put(endpoint);
1373 +
1374 +       return ret;
1375 +}
1376 +
1377 +static int pivariety_probe(struct i2c_client *client,
1378 +                          const struct i2c_device_id *id)
1379 +{
1380 +       struct device *dev = &client->dev;
1381 +       struct pivariety *pivariety;
1382 +       u32 device_id, firmware_version;
1383 +       int ret;
1384 +
1385 +       pivariety = devm_kzalloc(&client->dev, sizeof(*pivariety), GFP_KERNEL);
1386 +       if (!pivariety)
1387 +               return -ENOMEM;
1388 +
1389 +       /* Initialize subdev */
1390 +       v4l2_i2c_subdev_init(&pivariety->sd, client,
1391 +                            &pivariety_subdev_ops);
1392 +
1393 +       if (pivariety_parse_dt(pivariety, dev))
1394 +               return -EINVAL;
1395 +
1396 +       /* Get system clock (xclk) */
1397 +       pivariety->xclk = devm_clk_get(dev, "xclk");
1398 +       if (IS_ERR(pivariety->xclk)) {
1399 +               dev_err(dev, "failed to get xclk\n");
1400 +               return PTR_ERR(pivariety->xclk);
1401 +       }
1402 +
1403 +       pivariety->xclk_freq = clk_get_rate(pivariety->xclk);
1404 +       if (pivariety->xclk_freq != 24000000) {
1405 +               dev_err(dev, "xclk frequency not supported: %d Hz\n",
1406 +                       pivariety->xclk_freq);
1407 +               return -EINVAL;
1408 +       }
1409 +
1410 +       ret = pivariety_get_regulators(pivariety);
1411 +       if (ret)
1412 +               return ret;
1413 +
1414 +       /* Request optional enable pin */
1415 +       pivariety->reset_gpio = devm_gpiod_get_optional(dev, "reset",
1416 +                                                       GPIOD_OUT_HIGH);
1417 +
1418 +       ret = pivariety_power_on(dev);
1419 +       if (ret)
1420 +               return ret;
1421 +
1422 +       ret = pivariety_read(pivariety, DEVICE_ID_REG, &device_id);
1423 +       if (ret || device_id != DEVICE_ID) {
1424 +               dev_err(dev, "probe failed\n");
1425 +               ret = -ENODEV;
1426 +               goto error_power_off;
1427 +       }
1428 +
1429 +       ret = pivariety_read(pivariety, DEVICE_VERSION_REG, &firmware_version);
1430 +       if (ret)
1431 +               dev_err(dev, "read firmware version failed\n");
1432 +
1433 +       dev_info(dev, "firmware version: 0x%04X\n", firmware_version);
1434 +
1435 +       if (pivariety_enum_pixformat(pivariety)) {
1436 +               dev_err(dev, "enum pixformat failed.\n");
1437 +               ret = -ENODEV;
1438 +               goto error_power_off;
1439 +       }
1440 +
1441 +       if (pivariety_enum_controls(pivariety)) {
1442 +               dev_err(dev, "enum controls failed.\n");
1443 +               ret = -ENODEV;
1444 +               goto error_power_off;
1445 +       }
1446 +
1447 +       /* Initialize subdev */
1448 +       pivariety->sd.internal_ops = &pivariety_internal_ops;
1449 +       pivariety->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1450 +       pivariety->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1451 +       /* Initialize source pad */
1452 +       pivariety->pad.flags = MEDIA_PAD_FL_SOURCE;
1453 +
1454 +       ret = media_entity_pads_init(&pivariety->sd.entity, 1, &pivariety->pad);
1455 +       if (ret)
1456 +               goto error_handler_free;
1457 +
1458 +       ret = v4l2_async_register_subdev_sensor(&pivariety->sd);
1459 +       if (ret < 0)
1460 +               goto error_media_entity;
1461 +
1462 +       pm_runtime_set_active(dev);
1463 +       pm_runtime_enable(dev);
1464 +       pm_runtime_idle(dev);
1465 +
1466 +       return 0;
1467 +
1468 +error_media_entity:
1469 +       media_entity_cleanup(&pivariety->sd.entity);
1470 +
1471 +error_handler_free:
1472 +       pivariety_free_controls(pivariety);
1473 +
1474 +error_power_off:
1475 +       pivariety_power_off(dev);
1476 +
1477 +       return ret;
1478 +}
1479 +
1480 +static int pivariety_remove(struct i2c_client *client)
1481 +{
1482 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
1483 +       struct pivariety *pivariety = to_pivariety(sd);
1484 +
1485 +       v4l2_async_unregister_subdev(sd);
1486 +       media_entity_cleanup(&sd->entity);
1487 +       pivariety_free_controls(pivariety);
1488 +
1489 +       pm_runtime_disable(&client->dev);
1490 +       pm_runtime_set_suspended(&client->dev);
1491 +
1492 +       return 0;
1493 +}
1494 +
1495 +static const struct dev_pm_ops pivariety_pm_ops = {
1496 +       SET_SYSTEM_SLEEP_PM_OPS(pivariety_suspend, pivariety_resume)
1497 +       SET_RUNTIME_PM_OPS(pivariety_power_off, pivariety_power_on, NULL)
1498 +};
1499 +
1500 +static const struct of_device_id arducam_pivariety_dt_ids[] = {
1501 +       { .compatible = "arducam,arducam-pivariety" },
1502 +       { /* sentinel */ }
1503 +};
1504 +MODULE_DEVICE_TABLE(of, arducam_pivariety_dt_ids);
1505 +
1506 +static struct i2c_driver arducam_pivariety_i2c_driver = {
1507 +       .driver = {
1508 +               .name = "arducam-pivariety",
1509 +               .of_match_table = arducam_pivariety_dt_ids,
1510 +               .pm = &pivariety_pm_ops,
1511 +       },
1512 +       .probe = pivariety_probe,
1513 +       .remove = pivariety_remove,
1514 +};
1515 +
1516 +module_i2c_driver(arducam_pivariety_i2c_driver);
1517 +
1518 +MODULE_AUTHOR("Lee Jackson <info@arducam.com>");
1519 +MODULE_DESCRIPTION("Arducam Pivariety v4l2 driver");
1520 +MODULE_LICENSE("GPL v2");
1521 --- /dev/null
1522 +++ b/drivers/media/i2c/arducam-pivariety.h
1523 @@ -0,0 +1,107 @@
1524 +/* SPDX-License-Identifier: GPL-2.0 */
1525 +#ifndef _ARDUCAM_PIVARIETY_H_
1526 +#define _ARDUCAM_PIVARIETY_H_
1527 +
1528 +#define DEVICE_REG_BASE                0x0100
1529 +#define PIXFORMAT_REG_BASE     0x0200
1530 +#define FORMAT_REG_BASE                0x0300
1531 +#define CTRL_REG_BASE          0x0400
1532 +#define IPC_REG_BASE           0x0600
1533 +
1534 +#define ARDUCAM_MODE_STANDBY           0x00
1535 +#define ARDUCAM_MODE_STREAMING         0x01
1536 +
1537 +#define MODE_SELECT_REG                (DEVICE_REG_BASE | 0x0000)
1538 +#define DEVICE_VERSION_REG     (DEVICE_REG_BASE | 0x0001)
1539 +#define SENSOR_ID_REG          (DEVICE_REG_BASE | 0x0002)
1540 +#define DEVICE_ID_REG          (DEVICE_REG_BASE | 0x0003)
1541 +#define SYSTEM_IDLE_REG                (DEVICE_REG_BASE | 0x0007)
1542 +
1543 +#define PIXFORMAT_INDEX_REG            (PIXFORMAT_REG_BASE | 0x0000)
1544 +#define PIXFORMAT_TYPE_REG             (PIXFORMAT_REG_BASE | 0x0001)
1545 +#define PIXFORMAT_ORDER_REG            (PIXFORMAT_REG_BASE | 0x0002)
1546 +#define MIPI_LANES_REG                 (PIXFORMAT_REG_BASE | 0x0003)
1547 +#define FLIPS_DONT_CHANGE_ORDER_REG    (PIXFORMAT_REG_BASE | 0x0004)
1548 +
1549 +#define RESOLUTION_INDEX_REG   (FORMAT_REG_BASE | 0x0000)
1550 +#define FORMAT_WIDTH_REG       (FORMAT_REG_BASE | 0x0001)
1551 +#define FORMAT_HEIGHT_REG      (FORMAT_REG_BASE | 0x0002)
1552 +
1553 +#define CTRL_INDEX_REG (CTRL_REG_BASE | 0x0000)
1554 +#define CTRL_ID_REG    (CTRL_REG_BASE | 0x0001)
1555 +#define CTRL_MIN_REG   (CTRL_REG_BASE | 0x0002)
1556 +#define CTRL_MAX_REG   (CTRL_REG_BASE | 0x0003)
1557 +#define CTRL_STEP_REG  (CTRL_REG_BASE | 0x0004)
1558 +#define CTRL_DEF_REG   (CTRL_REG_BASE | 0x0005)
1559 +#define CTRL_VALUE_REG (CTRL_REG_BASE | 0x0006)
1560 +
1561 +#define IPC_SEL_TARGET_REG     (IPC_REG_BASE | 0x0000)
1562 +#define IPC_SEL_TOP_REG                (IPC_REG_BASE | 0x0001)
1563 +#define IPC_SEL_LEFT_REG       (IPC_REG_BASE | 0x0002)
1564 +#define IPC_SEL_WIDTH_REG      (IPC_REG_BASE | 0x0003)
1565 +#define IPC_SEL_HEIGHT_REG     (IPC_REG_BASE | 0x0004)
1566 +#define IPC_DELAY_REG          (IPC_REG_BASE | 0x0005)
1567 +
1568 +#define NO_DATA_AVAILABLE      0xFFFFFFFE
1569 +
1570 +#define DEVICE_ID      0x0030
1571 +
1572 +#define I2C_READ_RETRY_COUNT   3
1573 +#define I2C_WRITE_RETRY_COUNT  2
1574 +
1575 +#define V4L2_CID_ARDUCAM_BASE          (V4L2_CID_USER_BASE + 0x1000)
1576 +#define V4L2_CID_ARDUCAM_EXT_TRI       (V4L2_CID_ARDUCAM_BASE + 1)
1577 +#define V4L2_CID_ARDUCAM_IRCUT         (V4L2_CID_ARDUCAM_BASE + 8)
1578 +
1579 +enum image_dt {
1580 +       IMAGE_DT_YUV420_8 = 0x18,
1581 +       IMAGE_DT_YUV420_10,
1582 +
1583 +       IMAGE_DT_YUV420CSPS_8 = 0x1C,
1584 +       IMAGE_DT_YUV420CSPS_10,
1585 +       IMAGE_DT_YUV422_8,
1586 +       IMAGE_DT_YUV422_10,
1587 +       IMAGE_DT_RGB444,
1588 +       IMAGE_DT_RGB555,
1589 +       IMAGE_DT_RGB565,
1590 +       IMAGE_DT_RGB666,
1591 +       IMAGE_DT_RGB888,
1592 +
1593 +       IMAGE_DT_RAW6 = 0x28,
1594 +       IMAGE_DT_RAW7,
1595 +       IMAGE_DT_RAW8,
1596 +       IMAGE_DT_RAW10,
1597 +       IMAGE_DT_RAW12,
1598 +       IMAGE_DT_RAW14,
1599 +};
1600 +
1601 +enum bayer_order {
1602 +       BAYER_ORDER_BGGR = 0,
1603 +       BAYER_ORDER_GBRG = 1,
1604 +       BAYER_ORDER_GRBG = 2,
1605 +       BAYER_ORDER_RGGB = 3,
1606 +       BAYER_ORDER_GRAY = 4,
1607 +};
1608 +
1609 +enum yuv_order {
1610 +       YUV_ORDER_YUYV = 0,
1611 +       YUV_ORDER_YVYU = 1,
1612 +       YUV_ORDER_UYVY = 2,
1613 +       YUV_ORDER_VYUY = 3,
1614 +};
1615 +
1616 +struct arducam_resolution {
1617 +       u32 width;
1618 +       u32 height;
1619 +};
1620 +
1621 +struct arducam_format {
1622 +       u32 index;
1623 +       u32 mbus_code;
1624 +       u32 bayer_order;
1625 +       u32 data_type;
1626 +       u32 num_resolution_set;
1627 +       struct arducam_resolution *resolution_set;
1628 +};
1629 +
1630 +#endif