git.librecmc.org Git - oweals/openwrt.git/blob

   1 From c09b42cb057ddf8bd20d57c6b0ffd94f3e15ea7c Mon Sep 17 00:00:00 2001
   2 From: Markus Proeller <markus.proeller@pieye.org>
   3 Date: Thu, 10 Oct 2019 19:12:36 +0200
   4 Subject: [PATCH] media: i2c: Add a driver for the Infineon IRS1125
   5  depth sensor
   6
   7 The Infineon IRS1125 is a time of flight depth sensor that
   8 has a CSI-2 interface.
   9
  10 Add a V4L2 subdevice driver for this device.
  11
  12 Signed-off-by: Markus Proeller <markus.proeller@pieye.org>
  13 ---
  14  drivers/media/i2c/Kconfig   |   12 +
  15  drivers/media/i2c/Makefile  |    1 +
  16  drivers/media/i2c/irs1125.c | 1112 +++++++++++++++++++++++++++++++++++
  17  drivers/media/i2c/irs1125.h |   61 ++
  18  4 files changed, 1186 insertions(+)
  19  create mode 100644 drivers/media/i2c/irs1125.c
  20  create mode 100644 drivers/media/i2c/irs1125.h
  21
  22 --- a/drivers/media/i2c/Kconfig
  23 +++ b/drivers/media/i2c/Kconfig
  24 @@ -813,6 +813,18 @@ config VIDEO_OV13858
  25           This is a Video4Linux2 sensor driver for the OmniVision
  26           OV13858 camera.
  27
  28 +config VIDEO_IRS1125
  29 +       tristate "Infineon IRS1125 sensor support"
  30 +       depends on I2C && VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
  31 +       depends on MEDIA_CAMERA_SUPPORT
  32 +       select V4L2_FWNODE
  33 +       help
  34 +         This is a Video4Linux2 sensor-level driver for the Infineon
  35 +         IRS1125 camera.
  36 +
  37 +         To compile this driver as a module, choose M here: the
  38 +         module will be called irs1125.
  39 +
  40  config VIDEO_VS6624
  41         tristate "ST VS6624 sensor support"
  42         depends on VIDEO_V4L2 && I2C
  43 --- a/drivers/media/i2c/Makefile
  44 +++ b/drivers/media/i2c/Makefile
  45 @@ -80,6 +80,7 @@ obj-$(CONFIG_VIDEO_OV772X) += ov772x.o
  46  obj-$(CONFIG_VIDEO_OV7740) += ov7740.o
  47  obj-$(CONFIG_VIDEO_OV9650) += ov9650.o
  48  obj-$(CONFIG_VIDEO_OV13858) += ov13858.o
  49 +obj-$(CONFIG_VIDEO_IRS1125) += irs1125.o
  50  obj-$(CONFIG_VIDEO_MT9M032) += mt9m032.o
  51  obj-$(CONFIG_VIDEO_MT9M111) += mt9m111.o
  52  obj-$(CONFIG_VIDEO_MT9P031) += mt9p031.o
  53 --- /dev/null
  54 +++ b/drivers/media/i2c/irs1125.c
  55 @@ -0,0 +1,1112 @@
  56 +// SPDX-License-Identifier: GPL-2.0
  57 +/*
  58 + * A V4L2 driver for Infineon IRS1125 TOF cameras.
  59 + * Copyright (C) 2018, pieye GmbH
  60 + *
  61 + * Based on V4L2 OmniVision OV5647 Image Sensor driver
  62 + * Copyright (C) 2016 Ramiro Oliveira <roliveir@synopsys.com>
  63 + *
  64 + * DT / fwnode changes, and GPIO control taken from ov5640.c
  65 + * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved.
  66 + * Copyright (C) 2014-2017 Mentor Graphics Inc.
  67 + *
  68 + */
  69 +
  70 +#include "irs1125.h"
  71 +#include <linux/clk.h>
  72 +#include <linux/delay.h>
  73 +#include <linux/gpio/consumer.h>
  74 +#include <linux/i2c.h>
  75 +#include <linux/init.h>
  76 +#include <linux/io.h>
  77 +#include <linux/module.h>
  78 +#include <linux/of_graph.h>
  79 +#include <linux/slab.h>
  80 +#include <linux/videodev2.h>
  81 +#include <linux/firmware.h>
  82 +#include <media/v4l2-device.h>
  83 +#include <media/v4l2-fwnode.h>
  84 +#include <media/v4l2-image-sizes.h>
  85 +#include <media/v4l2-mediabus.h>
  86 +#include <media/v4l2-ctrls.h>
  87 +
  88 +#define CHECK_BIT(val, pos) ((val) & BIT(pos))
  89 +
  90 +#define SENSOR_NAME "irs1125"
  91 +
  92 +#define RESET_ACTIVE_DELAY_MS   20
  93 +
  94 +#define IRS1125_ALTERNATE_FW "irs1125_af.bin"
  95 +
  96 +#define IRS1125_REG_CSICFG       0xA882
  97 +#define IRS1125_REG_DESIGN_STEP         0xB0AD
  98 +#define IRS1125_REG_EFUSEVAL2   0xB09F
  99 +#define IRS1125_REG_EFUSEVAL3   0xB0A0
 100 +#define IRS1125_REG_EFUSEVAL4   0xB0A1
 101 +#define IRS1125_REG_DMEM_SHADOW         0xC320
 102 +
 103 +#define IRS1125_DESIGN_STEP_EXPECTED 0x0a12
 104 +
 105 +#define IRS1125_ROW_START_DEF          0
 106 +#define IRS1125_COLUMN_START_DEF       0
 107 +#define IRS1125_WINDOW_HEIGHT_DEF       288
 108 +#define IRS1125_WINDOW_WIDTH_DEF       352
 109 +
 110 +struct regval_list {
 111 +       u16 addr;
 112 +       u16 data;
 113 +};
 114 +
 115 +struct irs1125 {
 116 +       struct v4l2_subdev sd;
 117 +       struct media_pad pad;
 118 +       /* the parsed DT endpoint info */
 119 +       struct v4l2_fwnode_endpoint ep;
 120 +
 121 +       struct clk *xclk;
 122 +       struct v4l2_ctrl_handler ctrl_handler;
 123 +
 124 +       /* To serialize asynchronus callbacks */
 125 +       struct mutex lock;
 126 +
 127 +       /* image data layout */
 128 +       unsigned int num_seq;
 129 +
 130 +       /* reset pin */
 131 +       struct gpio_desc *reset;
 132 +
 133 +       /* V4l2 Controls to grab */
 134 +       struct v4l2_ctrl *ctrl_modplls;
 135 +       struct v4l2_ctrl *ctrl_numseq;
 136 +
 137 +       int power_count;
 138 +};
 139 +
 140 +static inline struct irs1125 *to_state(struct v4l2_subdev *sd)
 141 +{
 142 +       return container_of(sd, struct irs1125, sd);
 143 +}
 144 +
 145 +static struct regval_list irs1125_26MHz[] = {
 146 +       {0xB017, 0x0413},
 147 +       {0xB086, 0x3535},
 148 +       {0xB0AE, 0xEF02},
 149 +       {0xA000, 0x0004},
 150 +       {0xFFFF, 100},
 151 +
 152 +       {0xB062, 0x6383},
 153 +       {0xB063, 0x55A8},
 154 +       {0xB068, 0x7628},
 155 +       {0xB069, 0x03E2},
 156 +
 157 +       {0xFFFF, 100},
 158 +       {0xB05A, 0x01C5},
 159 +       {0xB05C, 0x0206},
 160 +       {0xB05D, 0x01C5},
 161 +       {0xB05F, 0x0206},
 162 +       {0xB016, 0x1335},
 163 +       {0xFFFF, 100},
 164 +       {0xA893, 0x8261},
 165 +       {0xA894, 0x89d8},
 166 +       {0xA895, 0x131d},
 167 +       {0xA896, 0x4251},
 168 +       {0xA897, 0x9D8A},
 169 +       {0xA898, 0x0BD8},
 170 +       {0xA899, 0x2245},
 171 +       {0xA89A, 0xAB9B},
 172 +       {0xA89B, 0x03B9},
 173 +       {0xA89C, 0x8041},
 174 +       {0xA89D, 0xE07E},
 175 +       {0xA89E, 0x0307},
 176 +       {0xFFFF, 100},
 177 +       {0xA88D, 0x0004},
 178 +       {0xA800, 0x0E68},
 179 +       {0xA801, 0x0000},
 180 +       {0xA802, 0x000C},
 181 +       {0xA803, 0x0000},
 182 +       {0xA804, 0x0E68},
 183 +       {0xA805, 0x0000},
 184 +       {0xA806, 0x0440},
 185 +       {0xA807, 0x0000},
 186 +       {0xA808, 0x0E68},
 187 +       {0xA809, 0x0000},
 188 +       {0xA80A, 0x0884},
 189 +       {0xA80B, 0x0000},
 190 +       {0xA80C, 0x0E68},
 191 +       {0xA80D, 0x0000},
 192 +       {0xA80E, 0x0CC8},
 193 +       {0xA80F, 0x0000},
 194 +       {0xA810, 0x0E68},
 195 +       {0xA811, 0x0000},
 196 +       {0xA812, 0x2000},
 197 +       {0xA813, 0x0000},
 198 +       {0xA882, 0x0081},
 199 +       {0xA88C, 0x403A},
 200 +       {0xA88F, 0x031E},
 201 +       {0xA892, 0x0351},
 202 +       {0x9813, 0x13FF},
 203 +       {0x981B, 0x7608},
 204 +
 205 +       {0xB008, 0x0000},
 206 +       {0xB015, 0x1513},
 207 +
 208 +       {0xFFFF, 100}
 209 +};
 210 +
 211 +static struct regval_list irs1125_seq_cfg[] = {
 212 +       {0xC3A0, 0x823D},
 213 +       {0xC3A1, 0xB13B},
 214 +       {0xC3A2, 0x0313},
 215 +       {0xC3A3, 0x4659},
 216 +       {0xC3A4, 0xC4EC},
 217 +       {0xC3A5, 0x03CE},
 218 +       {0xC3A6, 0x4259},
 219 +       {0xC3A7, 0xC4EC},
 220 +       {0xC3A8, 0x03CE},
 221 +       {0xC3A9, 0x8839},
 222 +       {0xC3AA, 0x89D8},
 223 +       {0xC3AB, 0x031D},
 224 +
 225 +       {0xC24C, 0x5529},
 226 +       {0xC24D, 0x0000},
 227 +       {0xC24E, 0x1200},
 228 +       {0xC24F, 0x6CB2},
 229 +       {0xC250, 0x0000},
 230 +       {0xC251, 0x5529},
 231 +       {0xC252, 0x42F4},
 232 +       {0xC253, 0xD1AF},
 233 +       {0xC254, 0x8A18},
 234 +       {0xC255, 0x0002},
 235 +       {0xC256, 0x5529},
 236 +       {0xC257, 0x6276},
 237 +       {0xC258, 0x11A7},
 238 +       {0xC259, 0xD907},
 239 +       {0xC25A, 0x0000},
 240 +       {0xC25B, 0x5529},
 241 +       {0xC25C, 0x07E0},
 242 +       {0xC25D, 0x7BFE},
 243 +       {0xC25E, 0x6402},
 244 +       {0xC25F, 0x0019},
 245 +
 246 +       {0xC3AC, 0x0007},
 247 +       {0xC3AD, 0xED88},
 248 +       {0xC320, 0x003E},
 249 +       {0xC321, 0x0000},
 250 +       {0xC322, 0x2000},
 251 +       {0xC323, 0x0000},
 252 +       {0xC324, 0x0271},
 253 +       {0xC325, 0x0000},
 254 +       {0xC326, 0x000C},
 255 +       {0xC327, 0x0000},
 256 +       {0xC328, 0x0271},
 257 +       {0xC329, 0x0000},
 258 +       {0xC32A, 0x0440},
 259 +       {0xC32B, 0x0000},
 260 +       {0xC32C, 0x0271},
 261 +       {0xC32D, 0x0000},
 262 +       {0xC32E, 0x0884},
 263 +       {0xC32F, 0x0000},
 264 +       {0xC330, 0x0271},
 265 +       {0xC331, 0x0000},
 266 +       {0xC332, 0x0CC8},
 267 +       {0xC333, 0x0000},
 268 +       {0xA88D, 0x0004},
 269 +
 270 +       {0xA890, 0x0000},
 271 +       {0xC219, 0x0002},
 272 +       {0xC21A, 0x0000},
 273 +       {0xC21B, 0x0000},
 274 +       {0xC21C, 0x00CD},
 275 +       {0xC21D, 0x0009},
 276 +       {0xC21E, 0x00CD},
 277 +       {0xC21F, 0x0009},
 278 +
 279 +       {0xA87C, 0x0000},
 280 +       {0xC032, 0x0001},
 281 +       {0xC034, 0x0000},
 282 +       {0xC035, 0x0001},
 283 +       {0xC039, 0x0000},
 284 +       {0xC401, 0x0002},
 285 +
 286 +       {0xFFFF, 1},
 287 +       {0xA87C, 0x0001}
 288 +};
 289 +
 290 +static int irs1125_write(struct v4l2_subdev *sd, u16 reg, u16 val)
 291 +{
 292 +       int ret;
 293 +       unsigned char data[4] = { reg >> 8, reg & 0xff, val >> 8, val & 0xff};
 294 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
 295 +
 296 +       ret = i2c_master_send(client, data, 4);
 297 +       if (ret < 0)
 298 +               dev_err(&client->dev, "%s: i2c write error, reg: %x\n",
 299 +                       __func__, reg);
 300 +
 301 +       return ret;
 302 +}
 303 +
 304 +static int irs1125_read(struct v4l2_subdev *sd, u16 reg, u16 *val)
 305 +{
 306 +       int ret;
 307 +       unsigned char data_w[2] = { reg >> 8, reg & 0xff };
 308 +       char rdval[2];
 309 +
 310 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
 311 +
 312 +       ret = i2c_master_send(client, data_w, 2);
 313 +       if (ret < 0) {
 314 +               dev_dbg(&client->dev, "%s: i2c write error, reg: %x\n",
 315 +                       __func__, reg);
 316 +               return ret;
 317 +       }
 318 +
 319 +       ret = i2c_master_recv(client, rdval, 2);
 320 +       if (ret < 0)
 321 +               dev_err(&client->dev, "%s: i2c read error, reg: %x\n",
 322 +                       __func__, reg);
 323 +
 324 +       *val = rdval[1] | (rdval[0] << 8);
 325 +
 326 +       return ret;
 327 +}
 328 +
 329 +static int irs1125_write_array(struct v4l2_subdev *sd,
 330 +                              struct regval_list *regs, int array_size)
 331 +{
 332 +       int i, ret;
 333 +
 334 +       for (i = 0; i < array_size; i++) {
 335 +               if (regs[i].addr == 0xFFFF) {
 336 +                       msleep(regs[i].data);
 337 +               } else {
 338 +                       ret = irs1125_write(sd, regs[i].addr, regs[i].data);
 339 +                       if (ret < 0)
 340 +                               return ret;
 341 +               }
 342 +       }
 343 +
 344 +       return 0;
 345 +}
 346 +
 347 +static int irs1125_stream_on(struct v4l2_subdev *sd)
 348 +{
 349 +       int ret;
 350 +       struct irs1125 *irs1125 = to_state(sd);
 351 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
 352 +
 353 +       v4l2_ctrl_grab(irs1125->ctrl_numseq, 1);
 354 +       v4l2_ctrl_grab(irs1125->ctrl_modplls, 1);
 355 +
 356 +       ret = irs1125_write(sd, 0xC400, 0x0001);
 357 +       if (ret < 0) {
 358 +               dev_err(&client->dev, "error enabling firmware: %d", ret);
 359 +               return ret;
 360 +       }
 361 +
 362 +       msleep(100);
 363 +
 364 +       return irs1125_write(sd, 0xA87C, 0x0001);
 365 +}
 366 +
 367 +static int irs1125_stream_off(struct v4l2_subdev *sd)
 368 +{
 369 +       int ret;
 370 +       struct irs1125 *irs1125 = to_state(sd);
 371 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
 372 +
 373 +       v4l2_ctrl_grab(irs1125->ctrl_numseq, 0);
 374 +       v4l2_ctrl_grab(irs1125->ctrl_modplls, 0);
 375 +
 376 +       ret = irs1125_write(sd, 0xA87C, 0x0000);
 377 +       if (ret < 0) {
 378 +               dev_err(&client->dev, "error disabling trigger: %d", ret);
 379 +               return ret;
 380 +       }
 381 +
 382 +       msleep(100);
 383 +
 384 +       return irs1125_write(sd, 0xC400, 0x0002);
 385 +}
 386 +
 387 +static int __sensor_init(struct v4l2_subdev *sd)
 388 +{
 389 +       unsigned int cnt, idx;
 390 +       int ret;
 391 +       u16 val;
 392 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
 393 +       struct irs1125 *irs1125 = to_state(sd);
 394 +       const struct firmware *fw;
 395 +       struct regval_list *reg_data;
 396 +
 397 +       cnt = 0;
 398 +       while (1) {
 399 +               ret = irs1125_read(sd, 0xC40F, &val);
 400 +               if (ret < 0) {
 401 +                       dev_err(&client->dev, "read register 0xC40F failed\n");
 402 +                       return ret;
 403 +               }
 404 +               if (CHECK_BIT(val, 14) == 0)
 405 +                       break;
 406 +
 407 +               if (cnt >= 5) {
 408 +                       dev_err(&client->dev, "timeout waiting for 0xC40F\n");
 409 +                       return -EAGAIN;
 410 +               }
 411 +
 412 +               cnt++;
 413 +       }
 414 +
 415 +       ret = irs1125_write_array(sd, irs1125_26MHz,
 416 +                                 ARRAY_SIZE(irs1125_26MHz));
 417 +       if (ret < 0) {
 418 +               dev_err(&client->dev, "write sensor default regs error\n");
 419 +               return ret;
 420 +       }
 421 +
 422 +       /* set CSI-2 number of data lanes */
 423 +       if (irs1125->ep.bus.mipi_csi2.num_data_lanes == 1) {
 424 +               val = 0x0001;
 425 +       } else if (irs1125->ep.bus.mipi_csi2.num_data_lanes == 2) {
 426 +               val = 0x0081;
 427 +       } else {
 428 +               dev_err(&client->dev, "invalid number of data lanes %d\n",
 429 +                       irs1125->ep.bus.mipi_csi2.num_data_lanes);
 430 +               return -EINVAL;
 431 +       }
 432 +
 433 +       ret = irs1125_write(sd, IRS1125_REG_CSICFG, val);
 434 +       if (ret < 0) {
 435 +               dev_err(&client->dev, "write sensor csi2 config error\n");
 436 +               return ret;
 437 +       }
 438 +
 439 +       /* request the firmware, this will block and timeout */
 440 +       ret = request_firmware(&fw, IRS1125_ALTERNATE_FW, &client->dev);
 441 +       if (ret) {
 442 +               dev_err(&client->dev,
 443 +                       "did not find the firmware file '%s' (status %d)\n",
 444 +                       IRS1125_ALTERNATE_FW, ret);
 445 +               return ret;
 446 +       }
 447 +
 448 +       if (fw->size % 4) {
 449 +               dev_err(&client->dev, "firmware file '%s' invalid\n",
 450 +                       IRS1125_ALTERNATE_FW);
 451 +               release_firmware(fw);
 452 +               return -EINVAL;
 453 +       }
 454 +
 455 +       for (idx = 0; idx < fw->size; idx += 4) {
 456 +               reg_data = (struct regval_list *)&fw->data[idx];
 457 +               ret = irs1125_write(sd, reg_data->addr, reg_data->data);
 458 +               if (ret < 0) {
 459 +                       dev_err(&client->dev, "firmware write error\n");
 460 +                       release_firmware(fw);
 461 +                       return ret;
 462 +               }
 463 +       }
 464 +       release_firmware(fw);
 465 +
 466 +       ret = irs1125_write_array(sd, irs1125_seq_cfg,
 467 +                                 ARRAY_SIZE(irs1125_seq_cfg));
 468 +       if (ret < 0) {
 469 +               dev_err(&client->dev, "write default sequence failed\n");
 470 +               return ret;
 471 +       }
 472 +
 473 +       return 0;
 474 +}
 475 +
 476 +static int irs1125_sensor_power(struct v4l2_subdev *sd, int on)
 477 +{
 478 +       int ret = 0;
 479 +       struct irs1125 *irs1125 = to_state(sd);
 480 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
 481 +
 482 +       mutex_lock(&irs1125->lock);
 483 +
 484 +       if (on && !irs1125->power_count) {
 485 +               gpiod_set_value_cansleep(irs1125->reset, 1);
 486 +               msleep(RESET_ACTIVE_DELAY_MS);
 487 +
 488 +               ret = clk_prepare_enable(irs1125->xclk);
 489 +               if (ret < 0) {
 490 +                       dev_err(&client->dev, "clk prepare enable failed\n");
 491 +                       goto out;
 492 +               }
 493 +
 494 +               ret = __sensor_init(sd);
 495 +               if (ret < 0) {
 496 +                       clk_disable_unprepare(irs1125->xclk);
 497 +                       dev_err(&client->dev,
 498 +                               "Camera not available, check Power\n");
 499 +                       goto out;
 500 +               }
 501 +       } else if (!on && irs1125->power_count == 1) {
 502 +               gpiod_set_value_cansleep(irs1125->reset, 0);
 503 +       }
 504 +
 505 +       /* Update the power count. */
 506 +       irs1125->power_count += on ? 1 : -1;
 507 +       WARN_ON(irs1125->power_count < 0);
 508 +
 509 +out:
 510 +       mutex_unlock(&irs1125->lock);
 511 +
 512 +       return ret;
 513 +}
 514 +
 515 +#ifdef CONFIG_VIDEO_ADV_DEBUG
 516 +static int irs1125_sensor_get_register(struct v4l2_subdev *sd,
 517 +                                      struct v4l2_dbg_register *reg)
 518 +{
 519 +       u16 val;
 520 +       int ret;
 521 +
 522 +       ret = irs1125_read(sd, reg->reg & 0xffff, &val);
 523 +       if (ret < 0)
 524 +               return ret;
 525 +
 526 +       reg->val = val;
 527 +       reg->size = 1;
 528 +
 529 +       return 0;
 530 +}
 531 +
 532 +static int irs1125_sensor_set_register(struct v4l2_subdev *sd,
 533 +                                      const struct v4l2_dbg_register *reg)
 534 +{
 535 +       return irs1125_write(sd, reg->reg & 0xffff, reg->val & 0xffff);
 536 +}
 537 +#endif
 538 +
 539 +static const struct v4l2_subdev_core_ops irs1125_subdev_core_ops = {
 540 +       .s_power = irs1125_sensor_power,
 541 +#ifdef CONFIG_VIDEO_ADV_DEBUG
 542 +       .g_register = irs1125_sensor_get_register,
 543 +       .s_register = irs1125_sensor_set_register,
 544 +#endif
 545 +};
 546 +
 547 +static int irs1125_s_stream(struct v4l2_subdev *sd, int enable)
 548 +{
 549 +       if (enable)
 550 +               return irs1125_stream_on(sd);
 551 +       else
 552 +               return irs1125_stream_off(sd);
 553 +}
 554 +
 555 +static const struct v4l2_subdev_video_ops irs1125_subdev_video_ops = {
 556 +       .s_stream = irs1125_s_stream,
 557 +};
 558 +
 559 +static int irs1125_enum_mbus_code(struct v4l2_subdev *sd,
 560 +                                 struct v4l2_subdev_pad_config *cfg,
 561 +       struct v4l2_subdev_mbus_code_enum *code)
 562 +{
 563 +       if (code->index > 0)
 564 +               return -EINVAL;
 565 +
 566 +       code->code = MEDIA_BUS_FMT_Y12_1X12;
 567 +
 568 +       return 0;
 569 +}
 570 +
 571 +static int irs1125_set_get_fmt(struct v4l2_subdev *sd,
 572 +                              struct v4l2_subdev_pad_config *cfg,
 573 +                              struct v4l2_subdev_format *format)
 574 +{
 575 +       struct v4l2_mbus_framefmt *fmt = &format->format;
 576 +       struct irs1125 *irs1125 = to_state(sd);
 577 +
 578 +       if (format->pad != 0)
 579 +               return -EINVAL;
 580 +
 581 +       /* Only one format is supported, so return that */
 582 +       memset(fmt, 0, sizeof(*fmt));
 583 +       fmt->code = MEDIA_BUS_FMT_Y12_1X12;
 584 +       fmt->colorspace = V4L2_COLORSPACE_RAW;
 585 +       fmt->field = V4L2_FIELD_NONE;
 586 +       fmt->width = IRS1125_WINDOW_WIDTH_DEF;
 587 +       fmt->height = IRS1125_WINDOW_HEIGHT_DEF * irs1125->num_seq;
 588 +
 589 +       return 0;
 590 +}
 591 +
 592 +static const struct v4l2_subdev_pad_ops irs1125_subdev_pad_ops = {
 593 +       .enum_mbus_code = irs1125_enum_mbus_code,
 594 +       .set_fmt = irs1125_set_get_fmt,
 595 +       .get_fmt = irs1125_set_get_fmt,
 596 +};
 597 +
 598 +static const struct v4l2_subdev_ops irs1125_subdev_ops = {
 599 +       .core = &irs1125_subdev_core_ops,
 600 +       .video = &irs1125_subdev_video_ops,
 601 +       .pad = &irs1125_subdev_pad_ops,
 602 +};
 603 +
 604 +static int irs1125_s_ctrl(struct v4l2_ctrl *ctrl)
 605 +{
 606 +       struct irs1125 *dev = container_of(ctrl->handler,
 607 +                                       struct irs1125, ctrl_handler);
 608 +       struct i2c_client *client = v4l2_get_subdevdata(&dev->sd);
 609 +       int err, i;
 610 +       struct irs1125_mod_pll *mod_cur, *mod_new;
 611 +       struct irs1125_seq_cfg *cfg_cur, *cfg_new;
 612 +       u16 addr, val;
 613 +
 614 +       err = 0;
 615 +
 616 +       switch (ctrl->id) {
 617 +       case IRS1125_CID_SAFE_RECONFIG:
 618 +       {
 619 +               struct irs1125_illu *illu_cur, *illu_new;
 620 +
 621 +               illu_new = (struct irs1125_illu *)ctrl->p_new.p;
 622 +               illu_cur = (struct irs1125_illu *)ctrl->p_cur.p;
 623 +               for (i = 0; i < IRS1125_NUM_SEQ_ENTRIES; i++) {
 624 +                       if (illu_cur[i].exposure != illu_new[i].exposure) {
 625 +                               addr = 0xA850 + i * 2;
 626 +                               val = illu_new[i].exposure;
 627 +                               err = irs1125_write(&dev->sd, addr, val);
 628 +                               if (err < 0)
 629 +                                       break;
 630 +                       }
 631 +                       if (illu_cur[i].framerate != illu_new[i].framerate) {
 632 +                               addr = 0xA851 + i * 2;
 633 +                               val = illu_new[i].framerate;
 634 +                               err = irs1125_write(&dev->sd, addr, val);
 635 +                               if (err < 0)
 636 +                                       break;
 637 +                       }
 638 +               }
 639 +               break;
 640 +       }
 641 +       case IRS1125_CID_MOD_PLL:
 642 +               mod_new = (struct irs1125_mod_pll *)ctrl->p_new.p;
 643 +               mod_cur = (struct irs1125_mod_pll *)ctrl->p_cur.p;
 644 +               for (i = 0; i < IRS1125_NUM_MOD_PLLS; i++) {
 645 +                       if (mod_cur[i].pllcfg1 != mod_new[i].pllcfg1) {
 646 +                               addr = 0xC3A0 + i * 3;
 647 +                               val = mod_new[i].pllcfg1;
 648 +                               err = irs1125_write(&dev->sd, addr, val);
 649 +                               if (err < 0)
 650 +                                       break;
 651 +                       }
 652 +                       if (mod_cur[i].pllcfg2 != mod_new[i].pllcfg2) {
 653 +                               addr = 0xC3A1 + i * 3;
 654 +                               val = mod_new[i].pllcfg2;
 655 +                               err = irs1125_write(&dev->sd, addr, val);
 656 +                               if (err < 0)
 657 +                                       break;
 658 +                       }
 659 +                       if (mod_cur[i].pllcfg3 != mod_new[i].pllcfg3) {
 660 +                               addr = 0xC3A2 + i * 3;
 661 +                               val = mod_new[i].pllcfg3;
 662 +                               err = irs1125_write(&dev->sd, addr, val);
 663 +                               if (err < 0)
 664 +                                       break;
 665 +                       }
 666 +                       if (mod_cur[i].pllcfg4 != mod_new[i].pllcfg4) {
 667 +                               addr = 0xC24C + i * 5;
 668 +                               val = mod_new[i].pllcfg4;
 669 +                               err = irs1125_write(&dev->sd, addr, val);
 670 +                               if (err < 0)
 671 +                                       break;
 672 +                       }
 673 +                       if (mod_cur[i].pllcfg5 != mod_new[i].pllcfg5) {
 674 +                               addr = 0xC24D + i * 5;
 675 +                               val = mod_new[i].pllcfg5;
 676 +                               err = irs1125_write(&dev->sd, addr, val);
 677 +                               if (err < 0)
 678 +                                       break;
 679 +                       }
 680 +                       if (mod_cur[i].pllcfg6 != mod_new[i].pllcfg6) {
 681 +                               addr = 0xC24E + i * 5;
 682 +                               val = mod_new[i].pllcfg6;
 683 +                               err = irs1125_write(&dev->sd, addr, val);
 684 +                               if (err < 0)
 685 +                                       break;
 686 +                       }
 687 +                       if (mod_cur[i].pllcfg7 != mod_new[i].pllcfg7) {
 688 +                               addr = 0xC24F + i * 5;
 689 +                               val = mod_new[i].pllcfg7;
 690 +                               err = irs1125_write(&dev->sd, addr, val);
 691 +                               if (err < 0)
 692 +                                       break;
 693 +                       }
 694 +                       if (mod_cur[i].pllcfg8 != mod_new[i].pllcfg8) {
 695 +                               addr = 0xC250 + i * 5;
 696 +                               val = mod_new[i].pllcfg8;
 697 +                               err = irs1125_write(&dev->sd, addr, val);
 698 +                               if (err < 0)
 699 +                                       break;
 700 +                       }
 701 +               }
 702 +               break;
 703 +       case IRS1125_CID_SEQ_CONFIG:
 704 +               cfg_new = (struct irs1125_seq_cfg *)ctrl->p_new.p;
 705 +               cfg_cur = (struct irs1125_seq_cfg *)ctrl->p_cur.p;
 706 +               for (i = 0; i < IRS1125_NUM_SEQ_ENTRIES; i++) {
 707 +                       if (cfg_cur[i].exposure != cfg_new[i].exposure) {
 708 +                               addr = IRS1125_REG_DMEM_SHADOW + i * 4;
 709 +                               val = cfg_new[i].exposure;
 710 +                               err = irs1125_write(&dev->sd, addr, val);
 711 +                               if (err < 0)
 712 +                                       break;
 713 +                       }
 714 +                       if (cfg_cur[i].framerate != cfg_new[i].framerate) {
 715 +                               addr = IRS1125_REG_DMEM_SHADOW + 1 + i * 4;
 716 +                               val = cfg_new[i].framerate;
 717 +                               err = irs1125_write(&dev->sd, addr, val);
 718 +                               if (err < 0)
 719 +                                       break;
 720 +                       }
 721 +                       if (cfg_cur[i].ps != cfg_new[i].ps) {
 722 +                               addr = IRS1125_REG_DMEM_SHADOW + 2 + i * 4;
 723 +                               val = cfg_new[i].ps;
 724 +                               err = irs1125_write(&dev->sd, addr, val);
 725 +                               if (err < 0)
 726 +                                       break;
 727 +                       }
 728 +                       if (cfg_cur[i].pll != cfg_new[i].pll) {
 729 +                               addr = IRS1125_REG_DMEM_SHADOW + 3 + i * 4;
 730 +                               val = cfg_new[i].pll;
 731 +                               err = irs1125_write(&dev->sd, addr, val);
 732 +                               if (err < 0)
 733 +                                       break;
 734 +                       }
 735 +               }
 736 +               break;
 737 +       case IRS1125_CID_NUM_SEQS:
 738 +               err = irs1125_write(&dev->sd, 0xA88D, ctrl->val - 1);
 739 +               if (err >= 0)
 740 +                       dev->num_seq = ctrl->val;
 741 +               break;
 742 +       case IRS1125_CID_CONTINUOUS_TRIG:
 743 +               if (ctrl->val == 0)
 744 +                       err = irs1125_write(&dev->sd, 0xA87C, 0);
 745 +               else
 746 +                       err = irs1125_write(&dev->sd, 0xA87C, 1);
 747 +               break;
 748 +       case IRS1125_CID_TRIGGER:
 749 +               if (ctrl->val != 0) {
 750 +                       err = irs1125_write(&dev->sd, 0xA87C, 1);
 751 +                       if (err >= 0)
 752 +                               err = irs1125_write(&dev->sd, 0xA87C, 0);
 753 +               }
 754 +               break;
 755 +       case IRS1125_CID_RECONFIG:
 756 +               if (ctrl->val != 0)
 757 +                       err = irs1125_write(&dev->sd, 0xA87A, 1);
 758 +               break;
 759 +       case IRS1125_CID_ILLU_ON:
 760 +               if (ctrl->val == 0)
 761 +                       err = irs1125_write(&dev->sd, 0xA892, 0x377);
 762 +               else
 763 +                       err = irs1125_write(&dev->sd, 0xA892, 0x355);
 764 +               break;
 765 +       default:
 766 +               break;
 767 +       }
 768 +
 769 +       if (err < 0)
 770 +               dev_err(&client->dev, "Error executing control ID: %d, val %d, err %d",
 771 +                       ctrl->id, ctrl->val, err);
 772 +       else
 773 +               err = 0;
 774 +
 775 +       return err;
 776 +}
 777 +
 778 +static const struct v4l2_ctrl_ops irs1125_ctrl_ops = {
 779 +       .s_ctrl = irs1125_s_ctrl,
 780 +};
 781 +
 782 +static const struct v4l2_ctrl_config irs1125_custom_ctrls[] = {
 783 +       {
 784 +               .ops = &irs1125_ctrl_ops,
 785 +               .id = IRS1125_CID_NUM_SEQS,
 786 +               .name = "Change number of sequences",
 787 +               .type = V4L2_CTRL_TYPE_INTEGER,
 788 +               .flags = V4L2_CTRL_FLAG_MODIFY_LAYOUT,
 789 +               .min = 1,
 790 +               .max = 20,
 791 +               .step = 1,
 792 +               .def = 5,
 793 +       }, {
 794 +               .ops = &irs1125_ctrl_ops,
 795 +               .id = IRS1125_CID_MOD_PLL,
 796 +               .name = "Reconfigure modulation PLLs",
 797 +               .type = V4L2_CTRL_TYPE_U16,
 798 +               .flags = V4L2_CTRL_FLAG_HAS_PAYLOAD,
 799 +               .min = 0,
 800 +               .max = U16_MAX,
 801 +               .step = 1,
 802 +               .def = 0,
 803 +               .elem_size = sizeof(u16),
 804 +               .dims = {sizeof(struct irs1125_mod_pll) / sizeof(u16),
 805 +                       IRS1125_NUM_MOD_PLLS}
 806 +       }, {
 807 +               .ops = &irs1125_ctrl_ops,
 808 +               .id = IRS1125_CID_SAFE_RECONFIG,
 809 +               .name = "Change exposure and pause of single seq",
 810 +               .type = V4L2_CTRL_TYPE_U16,
 811 +               .flags = V4L2_CTRL_FLAG_HAS_PAYLOAD,
 812 +               .min = 0,
 813 +               .max = U16_MAX,
 814 +               .step = 1,
 815 +               .def = 0,
 816 +               .elem_size = sizeof(u16),
 817 +               .dims = {sizeof(struct irs1125_illu) / sizeof(u16),
 818 +                       IRS1125_NUM_SEQ_ENTRIES}
 819 +       }, {
 820 +               .ops = &irs1125_ctrl_ops,
 821 +               .id = IRS1125_CID_SEQ_CONFIG,
 822 +               .name = "Change sequence settings",
 823 +               .type = V4L2_CTRL_TYPE_U16,
 824 +               .flags = V4L2_CTRL_FLAG_HAS_PAYLOAD,
 825 +               .min = 0,
 826 +               .max = U16_MAX,
 827 +               .step = 1,
 828 +               .def = 0,
 829 +               .elem_size = sizeof(u16),
 830 +               .dims = {sizeof(struct irs1125_seq_cfg) / sizeof(u16),
 831 +                       IRS1125_NUM_SEQ_ENTRIES}
 832 +       }, {
 833 +               .ops = &irs1125_ctrl_ops,
 834 +               .id = IRS1125_CID_CONTINUOUS_TRIG,
 835 +               .name = "Enable/disable continuous trigger",
 836 +               .type = V4L2_CTRL_TYPE_BOOLEAN,
 837 +               .flags = V4L2_CTRL_FLAG_EXECUTE_ON_WRITE,
 838 +               .min = 0,
 839 +               .max = 1,
 840 +               .step = 1,
 841 +               .def = 0
 842 +       }, {
 843 +               .ops = &irs1125_ctrl_ops,
 844 +               .id = IRS1125_CID_TRIGGER,
 845 +               .name = "Capture a single sequence",
 846 +               .type = V4L2_CTRL_TYPE_BOOLEAN,
 847 +               .flags = V4L2_CTRL_FLAG_EXECUTE_ON_WRITE,
 848 +               .min = 0,
 849 +               .max = 1,
 850 +               .step = 1,
 851 +               .def = 0
 852 +       }, {
 853 +               .ops = &irs1125_ctrl_ops,
 854 +               .id = IRS1125_CID_RECONFIG,
 855 +               .name = "Trigger imager reconfiguration",
 856 +               .type = V4L2_CTRL_TYPE_BOOLEAN,
 857 +               .flags = V4L2_CTRL_FLAG_EXECUTE_ON_WRITE,
 858 +               .min = 0,
 859 +               .max = 1,
 860 +               .step = 1,
 861 +               .def = 0
 862 +       }, {
 863 +               .ops = &irs1125_ctrl_ops,
 864 +               .id = IRS1125_CID_ILLU_ON,
 865 +               .name = "Turn illu on or off",
 866 +               .type = V4L2_CTRL_TYPE_BOOLEAN,
 867 +               .flags = V4L2_CTRL_FLAG_EXECUTE_ON_WRITE,
 868 +               .min = 0,
 869 +               .max = 1,
 870 +               .step = 1,
 871 +               .def = 1
 872 +       }, {
 873 +               .ops = &irs1125_ctrl_ops,
 874 +               .id = IRS1125_CID_IDENT0,
 875 +               .name = "Get ident 0 information",
 876 +               .type = V4L2_CTRL_TYPE_INTEGER,
 877 +               .flags = V4L2_CTRL_FLAG_READ_ONLY,
 878 +               .min = S32_MIN,
 879 +               .max = S32_MAX,
 880 +               .step = 1,
 881 +               .def = 0
 882 +       }, {
 883 +               .ops = &irs1125_ctrl_ops,
 884 +               .id = IRS1125_CID_IDENT1,
 885 +               .name = "Get ident 1 information",
 886 +               .type = V4L2_CTRL_TYPE_INTEGER,
 887 +               .flags = V4L2_CTRL_FLAG_READ_ONLY,
 888 +               .min = S32_MIN,
 889 +               .max = S32_MAX,
 890 +               .step = 1,
 891 +               .def = 0
 892 +       }, {
 893 +               .ops = &irs1125_ctrl_ops,
 894 +               .id = IRS1125_CID_IDENT2,
 895 +               .name = "Get ident 2 information",
 896 +               .type = V4L2_CTRL_TYPE_INTEGER,
 897 +               .flags = V4L2_CTRL_FLAG_READ_ONLY,
 898 +               .min = S32_MIN,
 899 +               .max = S32_MAX,
 900 +               .step = 1,
 901 +               .def = 0
 902 +       }
 903 +};
 904 +
 905 +static int irs1125_detect(struct v4l2_subdev *sd)
 906 +{
 907 +       u16 read;
 908 +       int ret;
 909 +       struct i2c_client *client = v4l2_get_subdevdata(sd);
 910 +
 911 +       ret = irs1125_read(sd, IRS1125_REG_DESIGN_STEP, &read);
 912 +       if (ret < 0) {
 913 +               dev_err(&client->dev, "error reading from i2c\n");
 914 +               return ret;
 915 +       }
 916 +
 917 +       if (read != IRS1125_DESIGN_STEP_EXPECTED) {
 918 +               dev_err(&client->dev, "Design step expected 0x%x got 0x%x",
 919 +                       IRS1125_DESIGN_STEP_EXPECTED, read);
 920 +               return -ENODEV;
 921 +       }
 922 +
 923 +       return 0;
 924 +}
 925 +
 926 +static int irs1125_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
 927 +{
 928 +       struct v4l2_mbus_framefmt *format =
 929 +       v4l2_subdev_get_try_format(sd, fh->pad, 0);
 930 +
 931 +       format->code = MEDIA_BUS_FMT_Y12_1X12;
 932 +       format->width = IRS1125_WINDOW_WIDTH_DEF;
 933 +       format->height = IRS1125_WINDOW_HEIGHT_DEF;
 934 +       format->field = V4L2_FIELD_NONE;
 935 +       format->colorspace = V4L2_COLORSPACE_RAW;
 936 +
 937 +       return 0;
 938 +}
 939 +
 940 +static const struct v4l2_subdev_internal_ops irs1125_subdev_internal_ops = {
 941 +       .open = irs1125_open,
 942 +};
 943 +
 944 +static int irs1125_ctrls_init(struct irs1125 *sensor, struct device *dev)
 945 +{
 946 +       struct v4l2_ctrl *ctrl;
 947 +       int err, i;
 948 +       struct v4l2_ctrl_handler *hdl;
 949 +
 950 +       hdl = &sensor->ctrl_handler;
 951 +       v4l2_ctrl_handler_init(hdl, ARRAY_SIZE(irs1125_custom_ctrls));
 952 +
 953 +       for (i = 0; i < ARRAY_SIZE(irs1125_custom_ctrls); i++)  {
 954 +               ctrl = v4l2_ctrl_new_custom(hdl, &irs1125_custom_ctrls[i],
 955 +                                           NULL);
 956 +               if (!ctrl)
 957 +                       dev_err(dev, "Failed to init custom control %s\n",
 958 +                               irs1125_custom_ctrls[i].name);
 959 +               else if (irs1125_custom_ctrls[i].id == IRS1125_CID_NUM_SEQS)
 960 +                       sensor->ctrl_numseq = ctrl;
 961 +               else if (irs1125_custom_ctrls[i].id == IRS1125_CID_MOD_PLL)
 962 +                       sensor->ctrl_modplls = ctrl;
 963 +       }
 964 +
 965 +       if (hdl->error) {
 966 +               dev_err(dev, "Error %d adding controls\n", hdl->error);
 967 +               err = hdl->error;
 968 +               goto error_ctrls;
 969 +       }
 970 +
 971 +       sensor->sd.ctrl_handler = hdl;
 972 +       return 0;
 973 +
 974 +error_ctrls:
 975 +       v4l2_ctrl_handler_free(&sensor->ctrl_handler);
 976 +       return -err;
 977 +}
 978 +
 979 +static int irs1125_ident_setup(struct irs1125 *sensor, struct device *dev)
 980 +{
 981 +       int ret;
 982 +       struct v4l2_ctrl *ctrl;
 983 +       struct v4l2_subdev *sd;
 984 +       u16 read;
 985 +
 986 +       sd = &sensor->sd;
 987 +
 988 +       ctrl = v4l2_ctrl_find(&sensor->ctrl_handler, IRS1125_CID_IDENT0);
 989 +       if (!ctrl) {
 990 +               dev_err(dev, "could not find device ctrl.\n");
 991 +               return -EINVAL;
 992 +       }
 993 +
 994 +       ret = irs1125_read(sd, IRS1125_REG_EFUSEVAL2, &read);
 995 +       if (ret < 0) {
 996 +               dev_err(dev, "error reading from i2c\n");
 997 +               return -EIO;
 998 +       }
 999 +
1000 +       v4l2_ctrl_s_ctrl(ctrl, read);
1001 +
1002 +       ctrl = v4l2_ctrl_find(&sensor->ctrl_handler, IRS1125_CID_IDENT1);
1003 +       if (!ctrl) {
1004 +               dev_err(dev, "could not find device ctrl.\n");
1005 +               return -EINVAL;
1006 +       }
1007 +
1008 +       ret = irs1125_read(sd, IRS1125_REG_EFUSEVAL3, &read);
1009 +       if (ret < 0) {
1010 +               dev_err(dev, "error reading from i2c\n");
1011 +               return -EIO;
1012 +       }
1013 +
1014 +       v4l2_ctrl_s_ctrl(ctrl, read);
1015 +
1016 +       ctrl = v4l2_ctrl_find(&sensor->ctrl_handler, IRS1125_CID_IDENT2);
1017 +       if (!ctrl) {
1018 +               dev_err(dev, "could not find device ctrl.\n");
1019 +               return -EINVAL;
1020 +       }
1021 +
1022 +       ret = irs1125_read(sd, IRS1125_REG_EFUSEVAL4, &read);
1023 +       if (ret < 0) {
1024 +               dev_err(dev, "error reading from i2c\n");
1025 +               return -EIO;
1026 +       }
1027 +       v4l2_ctrl_s_ctrl(ctrl, read & 0xFFFC);
1028 +
1029 +       return 0;
1030 +}
1031 +
1032 +static int irs1125_probe(struct i2c_client *client,
1033 +                        const struct i2c_device_id *id)
1034 +{
1035 +       struct device *dev = &client->dev;
1036 +       struct irs1125 *sensor;
1037 +       int ret;
1038 +       struct fwnode_handle *endpoint;
1039 +       u32 xclk_freq;
1040 +       int gpio_num;
1041 +
1042 +       sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
1043 +       if (!sensor)
1044 +               return -ENOMEM;
1045 +
1046 +       v4l2_i2c_subdev_init(&sensor->sd, client, &irs1125_subdev_ops);
1047 +
1048 +       /* Get CSI2 bus config */
1049 +       endpoint = fwnode_graph_get_next_endpoint(dev_fwnode(dev),
1050 +                                                 NULL);
1051 +       if (!endpoint) {
1052 +               dev_err(dev, "endpoint node not found\n");
1053 +               return -EINVAL;
1054 +       }
1055 +
1056 +       ret = v4l2_fwnode_endpoint_parse(endpoint, &sensor->ep);
1057 +       fwnode_handle_put(endpoint);
1058 +       if (ret) {
1059 +               dev_err(dev, "Could not parse endpoint\n");
1060 +               return ret;
1061 +       }
1062 +
1063 +       /* get system clock (xclk) */
1064 +       sensor->xclk = devm_clk_get(dev, NULL);
1065 +       if (IS_ERR(sensor->xclk)) {
1066 +               dev_err(dev, "could not get xclk");
1067 +               return PTR_ERR(sensor->xclk);
1068 +       }
1069 +
1070 +       xclk_freq = clk_get_rate(sensor->xclk);
1071 +       if (xclk_freq != 26000000) {
1072 +               dev_err(dev, "Unsupported clock frequency: %u\n", xclk_freq);
1073 +               return -EINVAL;
1074 +       }
1075 +
1076 +       sensor->num_seq = 5;
1077 +
1078 +       /* Request the power down GPIO */
1079 +       sensor->reset = devm_gpiod_get(&client->dev, "pwdn",
1080 +                                      GPIOD_OUT_LOW);
1081 +
1082 +       if (IS_ERR(sensor->reset)) {
1083 +               dev_err(dev, "could not get reset");
1084 +               return PTR_ERR(sensor->reset);
1085 +       }
1086 +
1087 +       gpio_num = desc_to_gpio(sensor->reset);
1088 +
1089 +       mutex_init(&sensor->lock);
1090 +
1091 +       ret = irs1125_ctrls_init(sensor, dev);
1092 +       if (ret < 0)
1093 +               goto mutex_remove;
1094 +
1095 +       sensor->sd.internal_ops = &irs1125_subdev_internal_ops;
1096 +       sensor->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1097 +       sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
1098 +       sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
1099 +       ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
1100 +       if (ret < 0)
1101 +               goto mutex_remove;
1102 +
1103 +       gpiod_set_value_cansleep(sensor->reset, 1);
1104 +       msleep(RESET_ACTIVE_DELAY_MS);
1105 +
1106 +       ret = irs1125_detect(&sensor->sd);
1107 +       if (ret < 0)
1108 +               goto error;
1109 +
1110 +       ret = irs1125_ident_setup(sensor, dev);
1111 +       if (ret < 0)
1112 +               goto error;
1113 +
1114 +       gpiod_set_value_cansleep(sensor->reset, 0);
1115 +
1116 +       ret = v4l2_async_register_subdev(&sensor->sd);
1117 +       if (ret < 0)
1118 +               goto error;
1119 +
1120 +       dev_dbg(dev, "Infineon IRS1125 camera driver probed\n");
1121 +
1122 +       return 0;
1123 +
1124 +error:
1125 +       media_entity_cleanup(&sensor->sd.entity);
1126 +mutex_remove:
1127 +       mutex_destroy(&sensor->lock);
1128 +       return ret;
1129 +}
1130 +
1131 +static int irs1125_remove(struct i2c_client *client)
1132 +{
1133 +       struct v4l2_subdev *sd = i2c_get_clientdata(client);
1134 +       struct irs1125 *irs1125 = to_state(sd);
1135 +
1136 +       v4l2_async_unregister_subdev(&irs1125->sd);
1137 +       media_entity_cleanup(&irs1125->sd.entity);
1138 +       v4l2_device_unregister_subdev(sd);
1139 +       mutex_destroy(&irs1125->lock);
1140 +       v4l2_ctrl_handler_free(&irs1125->ctrl_handler);
1141 +
1142 +       return 0;
1143 +}
1144 +
1145 +#if IS_ENABLED(CONFIG_OF)
1146 +static const struct of_device_id irs1125_of_match[] = {
1147 +       { .compatible = "infineon,irs1125" },
1148 +       { /* sentinel */ },
1149 +};
1150 +MODULE_DEVICE_TABLE(of, irs1125_of_match);
1151 +#endif
1152 +
1153 +static struct i2c_driver irs1125_driver = {
1154 +       .driver = {
1155 +               .of_match_table = of_match_ptr(irs1125_of_match),
1156 +               .name    = SENSOR_NAME,
1157 +       },
1158 +       .probe          = irs1125_probe,
1159 +       .remove         = irs1125_remove,
1160 +};
1161 +
1162 +module_i2c_driver(irs1125_driver);
1163 +
1164 +MODULE_AUTHOR("Markus Proeller <markus.proeller@pieye.org>");
1165 +MODULE_DESCRIPTION("Infineon irs1125 sensor driver");
1166 +MODULE_LICENSE("GPL v2");
1167 +
1168 --- /dev/null
1169 +++ b/drivers/media/i2c/irs1125.h
1170 @@ -0,0 +1,61 @@
1171 +/* SPDX-License-Identifier: GPL-2.0 */
1172 +/*
1173 + * A V4L2 driver for Infineon IRS1125 TOF cameras.
1174 + * Copyright (C) 2018, pieye GmbH
1175 + *
1176 + * Based on V4L2 OmniVision OV5647 Image Sensor driver
1177 + * Copyright (C) 2016 Ramiro Oliveira <roliveir@synopsys.com>
1178 + *
1179 + * DT / fwnode changes, and GPIO control taken from ov5640.c
1180 + * Copyright (C) 2011-2013 Freescale Semiconductor, Inc. All Rights Reserved.
1181 + * Copyright (C) 2014-2017 Mentor Graphics Inc.
1182 + *
1183 + */
1184 +
1185 +#ifndef IRS1125_H
1186 +#define IRS1125_H
1187 +
1188 +#include <linux/v4l2-controls.h>
1189 +#include <linux/types.h>
1190 +
1191 +#define IRS1125_NUM_SEQ_ENTRIES 20
1192 +#define IRS1125_NUM_MOD_PLLS 4
1193 +
1194 +#define IRS1125_CID_CUSTOM_BASE        (V4L2_CID_USER_BASE | 0xf000)
1195 +#define IRS1125_CID_SAFE_RECONFIG      (IRS1125_CID_CUSTOM_BASE + 0)
1196 +#define IRS1125_CID_CONTINUOUS_TRIG    (IRS1125_CID_CUSTOM_BASE + 1)
1197 +#define IRS1125_CID_TRIGGER            (IRS1125_CID_CUSTOM_BASE + 2)
1198 +#define IRS1125_CID_RECONFIG           (IRS1125_CID_CUSTOM_BASE + 3)
1199 +#define IRS1125_CID_ILLU_ON            (IRS1125_CID_CUSTOM_BASE + 4)
1200 +#define IRS1125_CID_NUM_SEQS           (IRS1125_CID_CUSTOM_BASE + 5)
1201 +#define IRS1125_CID_MOD_PLL            (IRS1125_CID_CUSTOM_BASE + 6)
1202 +#define IRS1125_CID_SEQ_CONFIG         (IRS1125_CID_CUSTOM_BASE + 7)
1203 +#define IRS1125_CID_IDENT0             (IRS1125_CID_CUSTOM_BASE + 8)
1204 +#define IRS1125_CID_IDENT1             (IRS1125_CID_CUSTOM_BASE + 9)
1205 +#define IRS1125_CID_IDENT2             (IRS1125_CID_CUSTOM_BASE + 10)
1206 +
1207 +struct irs1125_seq_cfg {
1208 +       __u16 exposure;
1209 +       __u16 framerate;
1210 +       __u16 ps;
1211 +       __u16 pll;
1212 +};
1213 +
1214 +struct irs1125_illu {
1215 +       __u16 exposure;
1216 +       __u16 framerate;
1217 +};
1218 +
1219 +struct irs1125_mod_pll {
1220 +       __u16 pllcfg1;
1221 +       __u16 pllcfg2;
1222 +       __u16 pllcfg3;
1223 +       __u16 pllcfg4;
1224 +       __u16 pllcfg5;
1225 +       __u16 pllcfg6;
1226 +       __u16 pllcfg7;
1227 +       __u16 pllcfg8;
1228 +};
1229 +
1230 +#endif /* IRS1125 */
1231 +