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

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