drivers/media/i2c/ds90ub953.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Driver for the Texas Instruments DS90UB953 video serializer
   4  *
   5  * Based on a driver from Luca Ceresoli <luca@lucaceresoli.net>
   6  *
   7  * Copyright (c) 2019 Luca Ceresoli <luca@lucaceresoli.net>
   8  * Copyright (c) 2023 Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>
   9  */
  10
  11 #include <linux/clk-provider.h>
  12 #include <linux/clk.h>
  13 #include <linux/delay.h>
  14 #include <linux/fwnode.h>
  15 #include <linux/gpio/driver.h>
  16 #include <linux/i2c-atr.h>
  17 #include <linux/i2c.h>
  18 #include <linux/kernel.h>
  19 #include <linux/math64.h>
  20 #include <linux/module.h>
  21 #include <linux/property.h>
  22 #include <linux/rational.h>
  23 #include <linux/regmap.h>
  24
  25 #include <media/i2c/ds90ub9xx.h>
  26 #include <media/v4l2-ctrls.h>
  27 #include <media/v4l2-event.h>
  28 #include <media/v4l2-subdev.h>
  29
  30 #define UB953_PAD_SINK                  0
  31 #define UB953_PAD_SOURCE                1
  32
  33 #define UB953_NUM_GPIOS                 4
  34
  35 #define UB953_REG_RESET_CTL                     0x01
  36 #define UB953_REG_RESET_CTL_DIGITAL_RESET_1     BIT(1)
  37 #define UB953_REG_RESET_CTL_DIGITAL_RESET_0     BIT(0)
  38
  39 #define UB953_REG_GENERAL_CFG                   0x02
  40 #define UB953_REG_GENERAL_CFG_CONT_CLK          BIT(6)
  41 #define UB953_REG_GENERAL_CFG_CSI_LANE_SEL_SHIFT        4
  42 #define UB953_REG_GENERAL_CFG_CSI_LANE_SEL_MASK GENMASK(5, 4)
  43 #define UB953_REG_GENERAL_CFG_CRC_TX_GEN_ENABLE BIT(1)
  44 #define UB953_REG_GENERAL_CFG_I2C_STRAP_MODE    BIT(0)
  45
  46 #define UB953_REG_MODE_SEL                      0x03
  47 #define UB953_REG_MODE_SEL_MODE_DONE            BIT(3)
  48 #define UB953_REG_MODE_SEL_MODE_OVERRIDE        BIT(4)
  49 #define UB953_REG_MODE_SEL_MODE_MASK            GENMASK(2, 0)
  50
  51 #define UB953_REG_CLKOUT_CTRL0                  0x06
  52 #define UB953_REG_CLKOUT_CTRL1                  0x07
  53
  54 #define UB953_REG_SCL_HIGH_TIME                 0x0b
  55 #define UB953_REG_SCL_LOW_TIME                  0x0c
  56
  57 #define UB953_REG_LOCAL_GPIO_DATA               0x0d
  58 #define UB953_REG_LOCAL_GPIO_DATA_GPIO_RMTEN(n)         BIT(4 + (n))
  59 #define UB953_REG_LOCAL_GPIO_DATA_GPIO_OUT_SRC(n)       BIT(0 + (n))
  60
  61 #define UB953_REG_GPIO_INPUT_CTRL               0x0e
  62 #define UB953_REG_GPIO_INPUT_CTRL_OUT_EN(n)     BIT(4 + (n))
  63 #define UB953_REG_GPIO_INPUT_CTRL_INPUT_EN(n)   BIT(0 + (n))
  64
  65 #define UB953_REG_REV_MASK_ID                   0x50
  66 #define UB953_REG_GENERAL_STATUS                0x52
  67
  68 #define UB953_REG_GPIO_PIN_STS                  0x53
  69 #define UB953_REG_GPIO_PIN_STS_GPIO_STS(n)      BIT(0 + (n))
  70
  71 #define UB953_REG_BIST_ERR_CNT                  0x54
  72 #define UB953_REG_CRC_ERR_CNT1                  0x55
  73 #define UB953_REG_CRC_ERR_CNT2                  0x56
  74
  75 #define UB953_REG_CSI_ERR_CNT                   0x5c
  76 #define UB953_REG_CSI_ERR_STATUS                0x5d
  77 #define UB953_REG_CSI_ERR_DLANE01               0x5e
  78 #define UB953_REG_CSI_ERR_DLANE23               0x5f
  79 #define UB953_REG_CSI_ERR_CLK_LANE              0x60
  80 #define UB953_REG_CSI_PKT_HDR_VC_ID             0x61
  81 #define UB953_REG_PKT_HDR_WC_LSB                0x62
  82 #define UB953_REG_PKT_HDR_WC_MSB                0x63
  83 #define UB953_REG_CSI_ECC                       0x64
  84
  85 #define UB953_REG_IND_ACC_CTL                   0xb0
  86 #define UB953_REG_IND_ACC_ADDR                  0xb1
  87 #define UB953_REG_IND_ACC_DATA                  0xb2
  88
  89 #define UB953_REG_FPD3_RX_ID(n)                 (0xf0 + (n))
  90 #define UB953_REG_FPD3_RX_ID_LEN                6
  91
  92 /* Indirect register blocks */
  93 #define UB953_IND_TARGET_PAT_GEN                0x00
  94 #define UB953_IND_TARGET_FPD3_TX                0x01
  95 #define UB953_IND_TARGET_DIE_ID                 0x02
  96
  97 #define UB953_IND_PGEN_CTL                      0x01
  98 #define UB953_IND_PGEN_CTL_PGEN_ENABLE          BIT(0)
  99 #define UB953_IND_PGEN_CFG                      0x02
 100 #define UB953_IND_PGEN_CSI_DI                   0x03
 101 #define UB953_IND_PGEN_LINE_SIZE1               0x04
 102 #define UB953_IND_PGEN_LINE_SIZE0               0x05
 103 #define UB953_IND_PGEN_BAR_SIZE1                0x06
 104 #define UB953_IND_PGEN_BAR_SIZE0                0x07
 105 #define UB953_IND_PGEN_ACT_LPF1                 0x08
 106 #define UB953_IND_PGEN_ACT_LPF0                 0x09
 107 #define UB953_IND_PGEN_TOT_LPF1                 0x0a
 108 #define UB953_IND_PGEN_TOT_LPF0                 0x0b
 109 #define UB953_IND_PGEN_LINE_PD1                 0x0c
 110 #define UB953_IND_PGEN_LINE_PD0                 0x0d
 111 #define UB953_IND_PGEN_VBP                      0x0e
 112 #define UB953_IND_PGEN_VFP                      0x0f
 113 #define UB953_IND_PGEN_COLOR(n)                 (0x10 + (n)) /* n <= 15 */
 114
 115 /* Note: Only sync mode supported for now */
 116 enum ub953_mode {
 117         /* FPD-Link III CSI-2 synchronous mode */
 118         UB953_MODE_SYNC,
 119         /* FPD-Link III CSI-2 non-synchronous mode, external ref clock */
 120         UB953_MODE_NONSYNC_EXT,
 121         /* FPD-Link III CSI-2 non-synchronous mode, internal ref clock */
 122         UB953_MODE_NONSYNC_INT,
 123         /* FPD-Link III DVP mode */
 124         UB953_MODE_DVP,
 125 };
 126
 127 struct ub953_hw_data {
 128         const char *model;
 129         bool is_ub971;
 130 };
 131
 132 struct ub953_data {
 133         const struct ub953_hw_data      *hw_data;
 134
 135         struct i2c_client       *client;
 136         struct regmap           *regmap;
 137
 138         u32                     num_data_lanes;
 139
 140         struct gpio_chip        gpio_chip;
 141
 142         struct v4l2_subdev      sd;
 143         struct media_pad        pads[2];
 144
 145         struct v4l2_async_notifier      notifier;
 146
 147         struct v4l2_subdev      *source_sd;
 148         u16                     source_sd_pad;
 149
 150         u64                     enabled_source_streams;
 151
 152         /* lock for register access */
 153         struct mutex            reg_lock;
 154
 155         u8                      current_indirect_target;
 156
 157         struct clk_hw           clkout_clk_hw;
 158
 159         enum ub953_mode         mode;
 160
 161         const struct ds90ub9xx_platform_data    *plat_data;
 162 };
 163
 164 static inline struct ub953_data *sd_to_ub953(struct v4l2_subdev *sd)
 165 {
 166         return container_of(sd, struct ub953_data, sd);
 167 }
 168
 169 /*
 170  * HW Access
 171  */
 172
 173 static int ub953_read(struct ub953_data *priv, u8 reg, u8 *val)
 174 {
 175         unsigned int v;
 176         int ret;
 177
 178         mutex_lock(&priv->reg_lock);
 179
 180         ret = regmap_read(priv->regmap, reg, &v);
 181         if (ret) {
 182                 dev_err(&priv->client->dev, "Cannot read register 0x%02x: %d\n",
 183                         reg, ret);
 184                 goto out_unlock;
 185         }
 186
 187         *val = v;
 188
 189 out_unlock:
 190         mutex_unlock(&priv->reg_lock);
 191
 192         return ret;
 193 }
 194
 195 static int ub953_write(struct ub953_data *priv, u8 reg, u8 val)
 196 {
 197         int ret;
 198
 199         mutex_lock(&priv->reg_lock);
 200
 201         ret = regmap_write(priv->regmap, reg, val);
 202         if (ret)
 203                 dev_err(&priv->client->dev,
 204                         "Cannot write register 0x%02x: %d\n", reg, ret);
 205
 206         mutex_unlock(&priv->reg_lock);
 207
 208         return ret;
 209 }
 210
 211 static int ub953_select_ind_reg_block(struct ub953_data *priv, u8 block)
 212 {
 213         struct device *dev = &priv->client->dev;
 214         int ret;
 215
 216         if (priv->current_indirect_target == block)
 217                 return 0;
 218
 219         ret = regmap_write(priv->regmap, UB953_REG_IND_ACC_CTL, block << 2);
 220         if (ret) {
 221                 dev_err(dev, "%s: cannot select indirect target %u (%d)\n",
 222                         __func__, block, ret);
 223                 return ret;
 224         }
 225
 226         priv->current_indirect_target = block;
 227
 228         return 0;
 229 }
 230
 231 __maybe_unused
 232 static int ub953_read_ind(struct ub953_data *priv, u8 block, u8 reg, u8 *val)
 233 {
 234         unsigned int v;
 235         int ret;
 236
 237         mutex_lock(&priv->reg_lock);
 238
 239         ret = ub953_select_ind_reg_block(priv, block);
 240         if (ret)
 241                 goto out_unlock;
 242
 243         ret = regmap_write(priv->regmap, UB953_REG_IND_ACC_ADDR, reg);
 244         if (ret) {
 245                 dev_err(&priv->client->dev,
 246                         "Write to IND_ACC_ADDR failed when reading %u:%x02x: %d\n",
 247                         block, reg, ret);
 248                 goto out_unlock;
 249         }
 250
 251         ret = regmap_read(priv->regmap, UB953_REG_IND_ACC_DATA, &v);
 252         if (ret) {
 253                 dev_err(&priv->client->dev,
 254                         "Write to IND_ACC_DATA failed when reading %u:%x02x: %d\n",
 255                         block, reg, ret);
 256                 goto out_unlock;
 257         }
 258
 259         *val = v;
 260
 261 out_unlock:
 262         mutex_unlock(&priv->reg_lock);
 263
 264         return ret;
 265 }
 266
 267 __maybe_unused
 268 static int ub953_write_ind(struct ub953_data *priv, u8 block, u8 reg, u8 val)
 269 {
 270         int ret;
 271
 272         mutex_lock(&priv->reg_lock);
 273
 274         ret = ub953_select_ind_reg_block(priv, block);
 275         if (ret)
 276                 goto out_unlock;
 277
 278         ret = regmap_write(priv->regmap, UB953_REG_IND_ACC_ADDR, reg);
 279         if (ret) {
 280                 dev_err(&priv->client->dev,
 281                         "Write to IND_ACC_ADDR failed when writing %u:%x02x: %d\n",
 282                         block, reg, ret);
 283                 goto out_unlock;
 284         }
 285
 286         ret = regmap_write(priv->regmap, UB953_REG_IND_ACC_DATA, val);
 287         if (ret) {
 288                 dev_err(&priv->client->dev,
 289                         "Write to IND_ACC_DATA failed when writing %u:%x02x\n: %d\n",
 290                         block, reg, ret);
 291         }
 292
 293 out_unlock:
 294         mutex_unlock(&priv->reg_lock);
 295
 296         return ret;
 297 }
 298
 299 /*
 300  * GPIO chip
 301  */
 302 static int ub953_gpio_get_direction(struct gpio_chip *gc, unsigned int offset)
 303 {
 304         struct ub953_data *priv = gpiochip_get_data(gc);
 305         int ret;
 306         u8 v;
 307
 308         ret = ub953_read(priv, UB953_REG_GPIO_INPUT_CTRL, &v);
 309         if (ret)
 310                 return ret;
 311
 312         if (v & UB953_REG_GPIO_INPUT_CTRL_INPUT_EN(offset))
 313                 return GPIO_LINE_DIRECTION_IN;
 314         else
 315                 return GPIO_LINE_DIRECTION_OUT;
 316 }
 317
 318 static int ub953_gpio_direction_in(struct gpio_chip *gc, unsigned int offset)
 319 {
 320         struct ub953_data *priv = gpiochip_get_data(gc);
 321
 322         return regmap_update_bits(priv->regmap, UB953_REG_GPIO_INPUT_CTRL,
 323                                   UB953_REG_GPIO_INPUT_CTRL_INPUT_EN(offset) |
 324                                           UB953_REG_GPIO_INPUT_CTRL_OUT_EN(offset),
 325                                   UB953_REG_GPIO_INPUT_CTRL_INPUT_EN(offset));
 326 }
 327
 328 static int ub953_gpio_direction_out(struct gpio_chip *gc, unsigned int offset,
 329                                     int value)
 330 {
 331         struct ub953_data *priv = gpiochip_get_data(gc);
 332         int ret;
 333
 334         ret = regmap_update_bits(priv->regmap, UB953_REG_LOCAL_GPIO_DATA,
 335                                  UB953_REG_LOCAL_GPIO_DATA_GPIO_OUT_SRC(offset),
 336                                  value ? UB953_REG_LOCAL_GPIO_DATA_GPIO_OUT_SRC(offset) :
 337                                          0);
 338
 339         if (ret)
 340                 return ret;
 341
 342         return regmap_update_bits(priv->regmap, UB953_REG_GPIO_INPUT_CTRL,
 343                                   UB953_REG_GPIO_INPUT_CTRL_INPUT_EN(offset) |
 344                                           UB953_REG_GPIO_INPUT_CTRL_OUT_EN(offset),
 345                                   UB953_REG_GPIO_INPUT_CTRL_OUT_EN(offset));
 346 }
 347
 348 static int ub953_gpio_get(struct gpio_chip *gc, unsigned int offset)
 349 {
 350         struct ub953_data *priv = gpiochip_get_data(gc);
 351         int ret;
 352         u8 v;
 353
 354         ret = ub953_read(priv, UB953_REG_GPIO_PIN_STS, &v);
 355         if (ret)
 356                 return ret;
 357
 358         return !!(v & UB953_REG_GPIO_PIN_STS_GPIO_STS(offset));
 359 }
 360
 361 static void ub953_gpio_set(struct gpio_chip *gc, unsigned int offset, int value)
 362 {
 363         struct ub953_data *priv = gpiochip_get_data(gc);
 364
 365         regmap_update_bits(priv->regmap, UB953_REG_LOCAL_GPIO_DATA,
 366                            UB953_REG_LOCAL_GPIO_DATA_GPIO_OUT_SRC(offset),
 367                            value ? UB953_REG_LOCAL_GPIO_DATA_GPIO_OUT_SRC(offset) :
 368                                    0);
 369 }
 370
 371 static int ub953_gpio_of_xlate(struct gpio_chip *gc,
 372                                const struct of_phandle_args *gpiospec,
 373                                u32 *flags)
 374 {
 375         if (flags)
 376                 *flags = gpiospec->args[1];
 377
 378         return gpiospec->args[0];
 379 }
 380
 381 static int ub953_gpiochip_probe(struct ub953_data *priv)
 382 {
 383         struct device *dev = &priv->client->dev;
 384         struct gpio_chip *gc = &priv->gpio_chip;
 385         int ret;
 386
 387         /* Set all GPIOs to local input mode */
 388         ub953_write(priv, UB953_REG_LOCAL_GPIO_DATA, 0);
 389         ub953_write(priv, UB953_REG_GPIO_INPUT_CTRL, 0xf);
 390
 391         gc->label = dev_name(dev);
 392         gc->parent = dev;
 393         gc->owner = THIS_MODULE;
 394         gc->base = -1;
 395         gc->can_sleep = true;
 396         gc->ngpio = UB953_NUM_GPIOS;
 397         gc->get_direction = ub953_gpio_get_direction;
 398         gc->direction_input = ub953_gpio_direction_in;
 399         gc->direction_output = ub953_gpio_direction_out;
 400         gc->get = ub953_gpio_get;
 401         gc->set = ub953_gpio_set;
 402         gc->of_xlate = ub953_gpio_of_xlate;
 403         gc->of_gpio_n_cells = 2;
 404
 405         ret = gpiochip_add_data(gc, priv);
 406         if (ret) {
 407                 dev_err(dev, "Failed to add GPIOs: %d\n", ret);
 408                 return ret;
 409         }
 410
 411         return 0;
 412 }
 413
 414 static void ub953_gpiochip_remove(struct ub953_data *priv)
 415 {
 416         gpiochip_remove(&priv->gpio_chip);
 417 }
 418
 419 /*
 420  * V4L2
 421  */
 422
 423 static int _ub953_set_routing(struct v4l2_subdev *sd,
 424                               struct v4l2_subdev_state *state,
 425                               struct v4l2_subdev_krouting *routing)
 426 {
 427         static const struct v4l2_mbus_framefmt format = {
 428                 .width = 640,
 429                 .height = 480,
 430                 .code = MEDIA_BUS_FMT_UYVY8_1X16,
 431                 .field = V4L2_FIELD_NONE,
 432                 .colorspace = V4L2_COLORSPACE_SRGB,
 433                 .ycbcr_enc = V4L2_YCBCR_ENC_601,
 434                 .quantization = V4L2_QUANTIZATION_LIM_RANGE,
 435                 .xfer_func = V4L2_XFER_FUNC_SRGB,
 436         };
 437         int ret;
 438
 439         /*
 440          * Note: we can only support up to V4L2_FRAME_DESC_ENTRY_MAX, until
 441          * frame desc is made dynamically allocated.
 442          */
 443
 444         if (routing->num_routes > V4L2_FRAME_DESC_ENTRY_MAX)
 445                 return -EINVAL;
 446
 447         ret = v4l2_subdev_routing_validate(sd, routing,
 448                                            V4L2_SUBDEV_ROUTING_ONLY_1_TO_1);
 449         if (ret)
 450                 return ret;
 451
 452         ret = v4l2_subdev_set_routing_with_fmt(sd, state, routing, &format);
 453         if (ret)
 454                 return ret;
 455
 456         return 0;
 457 }
 458
 459 static int ub953_set_routing(struct v4l2_subdev *sd,
 460                              struct v4l2_subdev_state *state,
 461                              enum v4l2_subdev_format_whence which,
 462                              struct v4l2_subdev_krouting *routing)
 463 {
 464         struct ub953_data *priv = sd_to_ub953(sd);
 465
 466         if (which == V4L2_SUBDEV_FORMAT_ACTIVE && priv->enabled_source_streams)
 467                 return -EBUSY;
 468
 469         return _ub953_set_routing(sd, state, routing);
 470 }
 471
 472 static int ub953_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
 473                                 struct v4l2_mbus_frame_desc *fd)
 474 {
 475         struct ub953_data *priv = sd_to_ub953(sd);
 476         struct v4l2_mbus_frame_desc source_fd;
 477         struct v4l2_subdev_route *route;
 478         struct v4l2_subdev_state *state;
 479         int ret;
 480
 481         if (pad != UB953_PAD_SOURCE)
 482                 return -EINVAL;
 483
 484         ret = v4l2_subdev_call(priv->source_sd, pad, get_frame_desc,
 485                                priv->source_sd_pad, &source_fd);
 486         if (ret)
 487                 return ret;
 488
 489         memset(fd, 0, sizeof(*fd));
 490
 491         fd->type = V4L2_MBUS_FRAME_DESC_TYPE_CSI2;
 492
 493         state = v4l2_subdev_lock_and_get_active_state(sd);
 494
 495         for_each_active_route(&state->routing, route) {
 496                 struct v4l2_mbus_frame_desc_entry *source_entry = NULL;
 497                 unsigned int i;
 498
 499                 if (route->source_pad != pad)
 500                         continue;
 501
 502                 for (i = 0; i < source_fd.num_entries; i++) {
 503                         if (source_fd.entry[i].stream == route->sink_stream) {
 504                                 source_entry = &source_fd.entry[i];
 505                                 break;
 506                         }
 507                 }
 508
 509                 if (!source_entry) {
 510                         dev_err(&priv->client->dev,
 511                                 "Failed to find stream from source frame desc\n");
 512                         ret = -EPIPE;
 513                         goto out_unlock;
 514                 }
 515
 516                 fd->entry[fd->num_entries].stream = route->source_stream;
 517                 fd->entry[fd->num_entries].flags = source_entry->flags;
 518                 fd->entry[fd->num_entries].length = source_entry->length;
 519                 fd->entry[fd->num_entries].pixelcode = source_entry->pixelcode;
 520                 fd->entry[fd->num_entries].bus.csi2.vc =
 521                         source_entry->bus.csi2.vc;
 522                 fd->entry[fd->num_entries].bus.csi2.dt =
 523                         source_entry->bus.csi2.dt;
 524
 525                 fd->num_entries++;
 526         }
 527
 528 out_unlock:
 529         v4l2_subdev_unlock_state(state);
 530
 531         return ret;
 532 }
 533
 534 static int ub953_set_fmt(struct v4l2_subdev *sd,
 535                          struct v4l2_subdev_state *state,
 536                          struct v4l2_subdev_format *format)
 537 {
 538         struct ub953_data *priv = sd_to_ub953(sd);
 539         struct v4l2_mbus_framefmt *fmt;
 540
 541         if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE &&
 542             priv->enabled_source_streams)
 543                 return -EBUSY;
 544
 545         /* No transcoding, source and sink formats must match. */
 546         if (format->pad == UB953_PAD_SOURCE)
 547                 return v4l2_subdev_get_fmt(sd, state, format);
 548
 549         /* Set sink format */
 550         fmt = v4l2_subdev_state_get_stream_format(state, format->pad,
 551                                                   format->stream);
 552         if (!fmt)
 553                 return -EINVAL;
 554
 555         *fmt = format->format;
 556
 557         /* Propagate to source format */
 558         fmt = v4l2_subdev_state_get_opposite_stream_format(state, format->pad,
 559                                                            format->stream);
 560         if (!fmt)
 561                 return -EINVAL;
 562
 563         *fmt = format->format;
 564
 565         return 0;
 566 }
 567
 568 static int ub953_init_cfg(struct v4l2_subdev *sd,
 569                           struct v4l2_subdev_state *state)
 570 {
 571         struct v4l2_subdev_route routes[] = {
 572                 {
 573                         .sink_pad = UB953_PAD_SINK,
 574                         .sink_stream = 0,
 575                         .source_pad = UB953_PAD_SOURCE,
 576                         .source_stream = 0,
 577                         .flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE,
 578                 },
 579         };
 580
 581         struct v4l2_subdev_krouting routing = {
 582                 .num_routes = ARRAY_SIZE(routes),
 583                 .routes = routes,
 584         };
 585
 586         return _ub953_set_routing(sd, state, &routing);
 587 }
 588
 589 static int ub953_log_status(struct v4l2_subdev *sd)
 590 {
 591         struct ub953_data *priv = sd_to_ub953(sd);
 592         struct device *dev = &priv->client->dev;
 593         u8 v = 0, v1 = 0, v2 = 0;
 594         unsigned int i;
 595         char id[UB953_REG_FPD3_RX_ID_LEN];
 596         u8 gpio_local_data;
 597         u8 gpio_input_ctrl;
 598         u8 gpio_pin_sts;
 599
 600         for (i = 0; i < sizeof(id); i++)
 601                 ub953_read(priv, UB953_REG_FPD3_RX_ID(i), &id[i]);
 602
 603         dev_info(dev, "ID '%.*s'\n", (int)sizeof(id), id);
 604
 605         ub953_read(priv, UB953_REG_GENERAL_STATUS, &v);
 606         dev_info(dev, "GENERAL_STATUS %#02x\n", v);
 607
 608         ub953_read(priv, UB953_REG_CRC_ERR_CNT1, &v1);
 609         ub953_read(priv, UB953_REG_CRC_ERR_CNT2, &v2);
 610         dev_info(dev, "CRC error count %u\n", v1 | (v2 << 8));
 611
 612         ub953_read(priv, UB953_REG_CSI_ERR_CNT, &v);
 613         dev_info(dev, "CSI error count %u\n", v);
 614
 615         ub953_read(priv, UB953_REG_CSI_ERR_STATUS, &v);
 616         dev_info(dev, "CSI_ERR_STATUS %#02x\n", v);
 617
 618         ub953_read(priv, UB953_REG_CSI_ERR_DLANE01, &v);
 619         dev_info(dev, "CSI_ERR_DLANE01 %#02x\n", v);
 620
 621         ub953_read(priv, UB953_REG_CSI_ERR_DLANE23, &v);
 622         dev_info(dev, "CSI_ERR_DLANE23 %#02x\n", v);
 623
 624         ub953_read(priv, UB953_REG_CSI_ERR_CLK_LANE, &v);
 625         dev_info(dev, "CSI_ERR_CLK_LANE %#02x\n", v);
 626
 627         ub953_read(priv, UB953_REG_CSI_PKT_HDR_VC_ID, &v);
 628         dev_info(dev, "CSI packet header VC %u ID %u\n", v >> 6, v & 0x3f);
 629
 630         ub953_read(priv, UB953_REG_PKT_HDR_WC_LSB, &v1);
 631         ub953_read(priv, UB953_REG_PKT_HDR_WC_MSB, &v2);
 632         dev_info(dev, "CSI packet header WC %u\n", (v2 << 8) | v1);
 633
 634         ub953_read(priv, UB953_REG_CSI_ECC, &v);
 635         dev_info(dev, "CSI ECC %#02x\n", v);
 636
 637         ub953_read(priv, UB953_REG_LOCAL_GPIO_DATA, &gpio_local_data);
 638         ub953_read(priv, UB953_REG_GPIO_INPUT_CTRL, &gpio_input_ctrl);
 639         ub953_read(priv, UB953_REG_GPIO_PIN_STS, &gpio_pin_sts);
 640
 641         for (i = 0; i < UB953_NUM_GPIOS; i++) {
 642                 dev_info(dev,
 643                          "GPIO%u: remote: %u is_input: %u is_output: %u val: %u sts: %u\n",
 644                          i,
 645                          !!(gpio_local_data & UB953_REG_LOCAL_GPIO_DATA_GPIO_RMTEN(i)),
 646                          !!(gpio_input_ctrl & UB953_REG_GPIO_INPUT_CTRL_INPUT_EN(i)),
 647                          !!(gpio_input_ctrl & UB953_REG_GPIO_INPUT_CTRL_OUT_EN(i)),
 648                          !!(gpio_local_data & UB953_REG_LOCAL_GPIO_DATA_GPIO_OUT_SRC(i)),
 649                          !!(gpio_pin_sts & UB953_REG_GPIO_PIN_STS_GPIO_STS(i)));
 650         }
 651
 652         return 0;
 653 }
 654
 655 static int ub953_enable_streams(struct v4l2_subdev *sd,
 656                                 struct v4l2_subdev_state *state, u32 pad,
 657                                 u64 streams_mask)
 658 {
 659         struct ub953_data *priv = sd_to_ub953(sd);
 660         u64 sink_streams;
 661         int ret;
 662
 663         sink_streams = v4l2_subdev_state_xlate_streams(state, UB953_PAD_SOURCE,
 664                                                        UB953_PAD_SINK,
 665                                                        &streams_mask);
 666
 667         ret = v4l2_subdev_enable_streams(priv->source_sd, priv->source_sd_pad,
 668                                          sink_streams);
 669         if (ret)
 670                 return ret;
 671
 672         priv->enabled_source_streams |= streams_mask;
 673
 674         return 0;
 675 }
 676
 677 static int ub953_disable_streams(struct v4l2_subdev *sd,
 678                                  struct v4l2_subdev_state *state, u32 pad,
 679                                  u64 streams_mask)
 680 {
 681         struct ub953_data *priv = sd_to_ub953(sd);
 682         u64 sink_streams;
 683         int ret;
 684
 685         sink_streams = v4l2_subdev_state_xlate_streams(state, UB953_PAD_SOURCE,
 686                                                        UB953_PAD_SINK,
 687                                                        &streams_mask);
 688
 689         ret = v4l2_subdev_disable_streams(priv->source_sd, priv->source_sd_pad,
 690                                           sink_streams);
 691         if (ret)
 692                 return ret;
 693
 694         priv->enabled_source_streams &= ~streams_mask;
 695
 696         return 0;
 697 }
 698
 699 static const struct v4l2_subdev_pad_ops ub953_pad_ops = {
 700         .enable_streams = ub953_enable_streams,
 701         .disable_streams = ub953_disable_streams,
 702         .set_routing = ub953_set_routing,
 703         .get_frame_desc = ub953_get_frame_desc,
 704         .get_fmt = v4l2_subdev_get_fmt,
 705         .set_fmt = ub953_set_fmt,
 706         .init_cfg = ub953_init_cfg,
 707 };
 708
 709 static const struct v4l2_subdev_core_ops ub953_subdev_core_ops = {
 710         .log_status = ub953_log_status,
 711         .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
 712         .unsubscribe_event = v4l2_event_subdev_unsubscribe,
 713 };
 714
 715 static const struct v4l2_subdev_ops ub953_subdev_ops = {
 716         .core = &ub953_subdev_core_ops,
 717         .pad = &ub953_pad_ops,
 718 };
 719
 720 static const struct media_entity_operations ub953_entity_ops = {
 721         .link_validate = v4l2_subdev_link_validate,
 722 };
 723
 724 static int ub953_notify_bound(struct v4l2_async_notifier *notifier,
 725                               struct v4l2_subdev *source_subdev,
 726                               struct v4l2_async_connection *asd)
 727 {
 728         struct ub953_data *priv = sd_to_ub953(notifier->sd);
 729         struct device *dev = &priv->client->dev;
 730         int ret;
 731
 732         ret = media_entity_get_fwnode_pad(&source_subdev->entity,
 733                                           source_subdev->fwnode,
 734                                           MEDIA_PAD_FL_SOURCE);
 735         if (ret < 0) {
 736                 dev_err(dev, "Failed to find pad for %s\n",
 737                         source_subdev->name);
 738                 return ret;
 739         }
 740
 741         priv->source_sd = source_subdev;
 742         priv->source_sd_pad = ret;
 743
 744         ret = media_create_pad_link(&source_subdev->entity, priv->source_sd_pad,
 745                                     &priv->sd.entity, 0,
 746                                     MEDIA_LNK_FL_ENABLED |
 747                                             MEDIA_LNK_FL_IMMUTABLE);
 748         if (ret) {
 749                 dev_err(dev, "Unable to link %s:%u -> %s:0\n",
 750                         source_subdev->name, priv->source_sd_pad,
 751                         priv->sd.name);
 752                 return ret;
 753         }
 754
 755         return 0;
 756 }
 757
 758 static const struct v4l2_async_notifier_operations ub953_notify_ops = {
 759         .bound = ub953_notify_bound,
 760 };
 761
 762 static int ub953_v4l2_notifier_register(struct ub953_data *priv)
 763 {
 764         struct device *dev = &priv->client->dev;
 765         struct v4l2_async_connection *asd;
 766         struct fwnode_handle *ep_fwnode;
 767         int ret;
 768
 769         ep_fwnode = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev),
 770                                                     UB953_PAD_SINK, 0, 0);
 771         if (!ep_fwnode) {
 772                 dev_err(dev, "No graph endpoint\n");
 773                 return -ENODEV;
 774         }
 775
 776         v4l2_async_subdev_nf_init(&priv->notifier, &priv->sd);
 777
 778         asd = v4l2_async_nf_add_fwnode_remote(&priv->notifier, ep_fwnode,
 779                                               struct v4l2_async_connection);
 780
 781         fwnode_handle_put(ep_fwnode);
 782
 783         if (IS_ERR(asd)) {
 784                 dev_err(dev, "Failed to add subdev: %ld", PTR_ERR(asd));
 785                 v4l2_async_nf_cleanup(&priv->notifier);
 786                 return PTR_ERR(asd);
 787         }
 788
 789         priv->notifier.ops = &ub953_notify_ops;
 790
 791         ret = v4l2_async_nf_register(&priv->notifier);
 792         if (ret) {
 793                 dev_err(dev, "Failed to register subdev_notifier");
 794                 v4l2_async_nf_cleanup(&priv->notifier);
 795                 return ret;
 796         }
 797
 798         return 0;
 799 }
 800
 801 static void ub953_v4l2_notifier_unregister(struct ub953_data *priv)
 802 {
 803         v4l2_async_nf_unregister(&priv->notifier);
 804         v4l2_async_nf_cleanup(&priv->notifier);
 805 }
 806
 807 /*
 808  * Probing
 809  */
 810
 811 static int ub953_i2c_master_init(struct ub953_data *priv)
 812 {
 813         /* i2c fast mode */
 814         u32 ref = 26250000;
 815         u32 scl_high = 915; /* ns */
 816         u32 scl_low = 1641; /* ns */
 817         int ret;
 818
 819         scl_high = div64_u64((u64)scl_high * ref, 1000000000) - 5;
 820         scl_low = div64_u64((u64)scl_low * ref, 1000000000) - 5;
 821
 822         ret = ub953_write(priv, UB953_REG_SCL_HIGH_TIME, scl_high);
 823         if (ret)
 824                 return ret;
 825
 826         ret = ub953_write(priv, UB953_REG_SCL_LOW_TIME, scl_low);
 827         if (ret)
 828                 return ret;
 829
 830         return 0;
 831 }
 832
 833 static u64 ub953_get_fc_rate(struct ub953_data *priv)
 834 {
 835         if (priv->mode != UB953_MODE_SYNC) {
 836                 /* Not supported */
 837                 return 0;
 838         }
 839
 840         if (priv->hw_data->is_ub971)
 841                 return priv->plat_data->bc_rate * 160ull;
 842         else
 843                 return priv->plat_data->bc_rate / 2 * 160ull;
 844 }
 845
 846 static unsigned long ub953_calc_clkout_ub953(struct ub953_data *priv,
 847                                              unsigned long target, u64 fc,
 848                                              u8 *hs_div, u8 *m, u8 *n)
 849 {
 850         /*
 851          * We always use 4 as a pre-divider (HS_CLK_DIV = 2).
 852          *
 853          * According to the datasheet:
 854          * - "HS_CLK_DIV typically should be set to either 16, 8, or 4 (default)."
 855          * - "if it is not possible to have an integer ratio of N/M, it is best to
 856          *    select a smaller value for HS_CLK_DIV.
 857          *
 858          * For above reasons the default HS_CLK_DIV seems the best in the average
 859          * case. Use always that value to keep the code simple.
 860          */
 861         static const unsigned long hs_clk_div = 4;
 862
 863         u64 fc_divided;
 864         unsigned long mul, div;
 865         unsigned long res;
 866
 867         /* clkout = fc / hs_clk_div * m / n */
 868
 869         fc_divided = div_u64(fc, hs_clk_div);
 870
 871         rational_best_approximation(target, fc_divided, (1 << 5) - 1,
 872                                     (1 << 8) - 1, &mul, &div);
 873
 874         res = div_u64(fc_divided * mul, div);
 875
 876         *hs_div = hs_clk_div;
 877         *m = mul;
 878         *n = div;
 879
 880         return res;
 881 }
 882
 883 static unsigned long ub953_calc_clkout_ub971(struct ub953_data *priv,
 884                                              unsigned long target, u64 fc,
 885                                              u8 *m, u8 *n)
 886 {
 887         u64 fc_divided;
 888         unsigned long mul, div;
 889         unsigned long res;
 890
 891         /* clkout = fc * m / (8 * n) */
 892
 893         fc_divided = div_u64(fc, 8);
 894
 895         rational_best_approximation(target, fc_divided, (1 << 5) - 1,
 896                                     (1 << 8) - 1, &mul, &div);
 897
 898         res = div_u64(fc_divided * mul, div);
 899
 900         *m = mul;
 901         *n = div;
 902
 903         return res;
 904 }
 905
 906 static unsigned long ub953_clkout_recalc_rate(struct clk_hw *hw,
 907                                               unsigned long parent_rate)
 908 {
 909         struct ub953_data *priv = container_of(hw, struct ub953_data, clkout_clk_hw);
 910         struct device *dev = &priv->client->dev;
 911         u8 ctrl0, ctrl1;
 912         u32 mul, div;
 913         u64 fc_rate;
 914         u32 hs_clk_div;
 915         u64 rate;
 916         int ret;
 917
 918         ret = ub953_read(priv, UB953_REG_CLKOUT_CTRL0, &ctrl0);
 919         if (ret) {
 920                 dev_err(dev, "Failed to read CLKOUT_CTRL0: %d\n", ret);
 921                 return 0;
 922         }
 923
 924         ret = ub953_read(priv, UB953_REG_CLKOUT_CTRL1, &ctrl1);
 925         if (ret) {
 926                 dev_err(dev, "Failed to read CLKOUT_CTRL1: %d\n", ret);
 927                 return 0;
 928         }
 929
 930         fc_rate = ub953_get_fc_rate(priv);
 931
 932         if (priv->hw_data->is_ub971) {
 933                 mul = ctrl0 & 0x1f;
 934                 div = ctrl1;
 935
 936                 if (div == 0)
 937                         return 0;
 938
 939                 rate = div_u64(fc_rate * mul, 8 * div);
 940
 941                 dev_dbg(dev, "clkout: fc rate %llu, mul %u, div %u = %llu\n",
 942                         fc_rate, mul, div, rate);
 943         } else {
 944                 mul = ctrl0 & 0x1f;
 945                 hs_clk_div = 1 << (ctrl0 >> 5);
 946                 div = ctrl1;
 947
 948                 if (div == 0)
 949                         return 0;
 950
 951                 rate = div_u64(div_u64(fc_rate, hs_clk_div) * mul, div);
 952
 953                 dev_dbg(dev,
 954                         "clkout: fc rate %llu, hs_clk_div %u, mul %u, div %u = %llu\n",
 955                         fc_rate, hs_clk_div, mul, div, rate);
 956         }
 957
 958         return rate;
 959 }
 960
 961 static long ub953_clkout_round_rate(struct clk_hw *hw, unsigned long rate,
 962                                     unsigned long *parent_rate)
 963 {
 964         struct ub953_data *priv = container_of(hw, struct ub953_data, clkout_clk_hw);
 965         struct device *dev = &priv->client->dev;
 966         unsigned long res;
 967         u64 fc_rate;
 968         u8 hs_div, m, n;
 969
 970         fc_rate = ub953_get_fc_rate(priv);
 971
 972         if (priv->hw_data->is_ub971) {
 973                 res = ub953_calc_clkout_ub971(priv, rate, fc_rate, &m, &n);
 974
 975                 dev_dbg(dev, "%s %llu * %u / (8 * %u) = %lu (requested %lu)",
 976                         __func__, fc_rate, m, n, res, rate);
 977         } else {
 978                 res = ub953_calc_clkout_ub953(priv, rate, fc_rate, &hs_div, &m, &n);
 979
 980                 dev_dbg(dev, "%s %llu / %u * %u / %u = %lu (requested %lu)",
 981                         __func__, fc_rate, hs_div, m, n, res, rate);
 982         }
 983
 984         return res;
 985 }
 986
 987 static int ub953_clkout_set_rate(struct clk_hw *hw, unsigned long rate,
 988                                  unsigned long parent_rate)
 989 {
 990         struct ub953_data *priv = container_of(hw, struct ub953_data, clkout_clk_hw);
 991         u64 fc_rate;
 992         u8 hs_div, m, n;
 993         unsigned long res;
 994
 995         fc_rate = ub953_get_fc_rate(priv);
 996
 997         if (priv->hw_data->is_ub971) {
 998                 res = ub953_calc_clkout_ub971(priv, rate, fc_rate, &m, &n);
 999
1000                 ub953_write(priv, UB953_REG_CLKOUT_CTRL0, m);
1001                 ub953_write(priv, UB953_REG_CLKOUT_CTRL1, n);
1002         } else {
1003                 res = ub953_calc_clkout_ub953(priv, rate, fc_rate, &hs_div, &m, &n);
1004
1005                 ub953_write(priv, UB953_REG_CLKOUT_CTRL0, (__ffs(hs_div) << 5) | m);
1006                 ub953_write(priv, UB953_REG_CLKOUT_CTRL1, n);
1007         }
1008
1009         dev_dbg(&priv->client->dev, "%s %lu (requested %lu)\n", __func__, res,
1010                 rate);
1011
1012         return 0;
1013 }
1014
1015 static const struct clk_ops ub953_clkout_ops = {
1016         .recalc_rate    = ub953_clkout_recalc_rate,
1017         .round_rate     = ub953_clkout_round_rate,
1018         .set_rate       = ub953_clkout_set_rate,
1019 };
1020
1021 static void ub953_init_clkout_ub953(struct ub953_data *priv)
1022 {
1023         u64 fc_rate;
1024         u8 hs_div, m, n;
1025
1026         fc_rate = ub953_get_fc_rate(priv);
1027
1028         ub953_calc_clkout_ub953(priv, 25000000, fc_rate, &hs_div, &m, &n);
1029
1030         ub953_write(priv, UB953_REG_CLKOUT_CTRL0, (__ffs(hs_div) << 5) | m);
1031         ub953_write(priv, UB953_REG_CLKOUT_CTRL1, n);
1032 }
1033
1034 static void ub953_init_clkout_ub971(struct ub953_data *priv)
1035 {
1036         u64 fc_rate;
1037         u8 m, n;
1038
1039         fc_rate = ub953_get_fc_rate(priv);
1040
1041         ub953_calc_clkout_ub971(priv, 25000000, fc_rate, &m, &n);
1042
1043         ub953_write(priv, UB953_REG_CLKOUT_CTRL0, m);
1044         ub953_write(priv, UB953_REG_CLKOUT_CTRL1, n);
1045 }
1046
1047 static int ub953_register_clkout(struct ub953_data *priv)
1048 {
1049         struct device *dev = &priv->client->dev;
1050         const struct clk_init_data init = {
1051                 .name = kasprintf(GFP_KERNEL, "ds90%s.%s.clk_out",
1052                                   priv->hw_data->model, dev_name(dev)),
1053                 .ops = &ub953_clkout_ops,
1054         };
1055         int ret;
1056
1057         if (!init.name)
1058                 return -ENOMEM;
1059
1060         /* Initialize clkout to 25MHz by default */
1061         if (priv->hw_data->is_ub971)
1062                 ub953_init_clkout_ub971(priv);
1063         else
1064                 ub953_init_clkout_ub953(priv);
1065
1066         priv->clkout_clk_hw.init = &init;
1067
1068         ret = devm_clk_hw_register(dev, &priv->clkout_clk_hw);
1069         kfree(init.name);
1070         if (ret)
1071                 return dev_err_probe(dev, ret, "Cannot register clock HW\n");
1072
1073         ret = devm_of_clk_add_hw_provider(dev, of_clk_hw_simple_get,
1074                                           &priv->clkout_clk_hw);
1075         if (ret)
1076                 return dev_err_probe(dev, ret,
1077                                      "Cannot add OF clock provider\n");
1078
1079         return 0;
1080 }
1081
1082 static int ub953_add_i2c_adapter(struct ub953_data *priv)
1083 {
1084         struct device *dev = &priv->client->dev;
1085         struct fwnode_handle *i2c_handle;
1086         int ret;
1087
1088         i2c_handle = device_get_named_child_node(dev, "i2c");
1089         if (!i2c_handle)
1090                 return 0;
1091
1092         ret = i2c_atr_add_adapter(priv->plat_data->atr, priv->plat_data->port,
1093                                   dev, i2c_handle);
1094
1095         fwnode_handle_put(i2c_handle);
1096
1097         if (ret)
1098                 return ret;
1099
1100         return 0;
1101 }
1102
1103 static const struct regmap_config ub953_regmap_config = {
1104         .name = "ds90ub953",
1105         .reg_bits = 8,
1106         .val_bits = 8,
1107         .reg_format_endian = REGMAP_ENDIAN_DEFAULT,
1108         .val_format_endian = REGMAP_ENDIAN_DEFAULT,
1109 };
1110
1111 static int ub953_parse_dt(struct ub953_data *priv)
1112 {
1113         struct device *dev = &priv->client->dev;
1114         struct fwnode_handle *ep_fwnode;
1115         int ret;
1116
1117         ep_fwnode = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev),
1118                                                     UB953_PAD_SINK, 0, 0);
1119         if (!ep_fwnode)
1120                 return dev_err_probe(dev, -ENOENT, "no endpoint found\n");
1121
1122         ret = fwnode_property_count_u32(ep_fwnode, "data-lanes");
1123
1124         fwnode_handle_put(ep_fwnode);
1125
1126         if (ret < 0)
1127                 return dev_err_probe(dev, ret,
1128                                      "failed to parse property 'data-lanes'\n");
1129
1130         if (ret != 1 && ret != 2 && ret != 4)
1131                 return dev_err_probe(dev, -EINVAL,
1132                                      "bad number of data-lanes: %d\n", ret);
1133
1134         priv->num_data_lanes = ret;
1135
1136         return 0;
1137 }
1138
1139 static int ub953_hw_init(struct ub953_data *priv)
1140 {
1141         struct device *dev = &priv->client->dev;
1142         bool mode_override;
1143         int ret;
1144         u8 v;
1145
1146         ret = ub953_read(priv, UB953_REG_MODE_SEL, &v);
1147         if (ret)
1148                 return ret;
1149
1150         if (!(v & UB953_REG_MODE_SEL_MODE_DONE))
1151                 return dev_err_probe(dev, -EIO, "Mode value not stabilized\n");
1152
1153         mode_override = v & UB953_REG_MODE_SEL_MODE_OVERRIDE;
1154
1155         switch (v & UB953_REG_MODE_SEL_MODE_MASK) {
1156         case 0:
1157                 priv->mode = UB953_MODE_SYNC;
1158                 break;
1159         case 2:
1160                 priv->mode = UB953_MODE_NONSYNC_EXT;
1161                 break;
1162         case 3:
1163                 priv->mode = UB953_MODE_NONSYNC_INT;
1164                 break;
1165         case 5:
1166                 priv->mode = UB953_MODE_DVP;
1167                 break;
1168         default:
1169                 return dev_err_probe(dev, -EIO,
1170                                      "Invalid mode in mode register\n");
1171         }
1172
1173         dev_dbg(dev, "mode from %s: %#x\n", mode_override ? "reg" : "strap",
1174                 priv->mode);
1175
1176         if (priv->mode != UB953_MODE_SYNC)
1177                 return dev_err_probe(dev, -ENODEV,
1178                                      "Only synchronous mode supported\n");
1179
1180         ret = ub953_read(priv, UB953_REG_REV_MASK_ID, &v);
1181         if (ret)
1182                 return dev_err_probe(dev, ret, "Failed to read revision");
1183
1184         dev_info(dev, "Found %s rev/mask %#04x\n", priv->hw_data->model, v);
1185
1186         ret = ub953_read(priv, UB953_REG_GENERAL_CFG, &v);
1187         if (ret)
1188                 return ret;
1189
1190         dev_dbg(dev, "i2c strap setting %s V\n",
1191                 (v & UB953_REG_GENERAL_CFG_I2C_STRAP_MODE) ? "1.8" : "3.3");
1192
1193         ret = ub953_i2c_master_init(priv);
1194         if (ret)
1195                 return dev_err_probe(dev, ret, "i2c init failed\n");
1196
1197         ub953_write(priv, UB953_REG_GENERAL_CFG,
1198                     UB953_REG_GENERAL_CFG_CONT_CLK |
1199                     ((priv->num_data_lanes - 1) << UB953_REG_GENERAL_CFG_CSI_LANE_SEL_SHIFT) |
1200                     UB953_REG_GENERAL_CFG_CRC_TX_GEN_ENABLE);
1201
1202         return 0;
1203 }
1204
1205 static int ub953_subdev_init(struct ub953_data *priv)
1206 {
1207         struct device *dev = &priv->client->dev;
1208         int ret;
1209
1210         v4l2_i2c_subdev_init(&priv->sd, priv->client, &ub953_subdev_ops);
1211
1212         priv->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
1213                           V4L2_SUBDEV_FL_HAS_EVENTS | V4L2_SUBDEV_FL_STREAMS;
1214         priv->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
1215         priv->sd.entity.ops = &ub953_entity_ops;
1216
1217         priv->pads[0].flags = MEDIA_PAD_FL_SINK;
1218         priv->pads[1].flags = MEDIA_PAD_FL_SOURCE;
1219
1220         ret = media_entity_pads_init(&priv->sd.entity, 2, priv->pads);
1221         if (ret)
1222                 return dev_err_probe(dev, ret, "Failed to init pads\n");
1223
1224         ret = v4l2_subdev_init_finalize(&priv->sd);
1225         if (ret)
1226                 goto err_entity_cleanup;
1227
1228         ret = ub953_v4l2_notifier_register(priv);
1229         if (ret) {
1230                 dev_err_probe(dev, ret,
1231                               "v4l2 subdev notifier register failed\n");
1232                 goto err_free_state;
1233         }
1234
1235         ret = v4l2_async_register_subdev(&priv->sd);
1236         if (ret) {
1237                 dev_err_probe(dev, ret, "v4l2_async_register_subdev error\n");
1238                 goto err_unreg_notif;
1239         }
1240
1241         return 0;
1242
1243 err_unreg_notif:
1244         ub953_v4l2_notifier_unregister(priv);
1245 err_free_state:
1246         v4l2_subdev_cleanup(&priv->sd);
1247 err_entity_cleanup:
1248         media_entity_cleanup(&priv->sd.entity);
1249
1250         return ret;
1251 }
1252
1253 static void ub953_subdev_uninit(struct ub953_data *priv)
1254 {
1255         v4l2_async_unregister_subdev(&priv->sd);
1256         ub953_v4l2_notifier_unregister(priv);
1257         v4l2_subdev_cleanup(&priv->sd);
1258         fwnode_handle_put(priv->sd.fwnode);
1259         media_entity_cleanup(&priv->sd.entity);
1260 }
1261
1262 static int ub953_probe(struct i2c_client *client)
1263 {
1264         struct device *dev = &client->dev;
1265         struct ub953_data *priv;
1266         int ret;
1267
1268         priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
1269         if (!priv)
1270                 return -ENOMEM;
1271
1272         priv->client = client;
1273
1274         priv->hw_data = device_get_match_data(dev);
1275
1276         priv->plat_data = dev_get_platdata(&client->dev);
1277         if (!priv->plat_data)
1278                 return dev_err_probe(dev, -ENODEV, "Platform data missing\n");
1279
1280         mutex_init(&priv->reg_lock);
1281
1282         /*
1283          * Initialize to invalid values so that the first reg writes will
1284          * configure the target.
1285          */
1286         priv->current_indirect_target = 0xff;
1287
1288         priv->regmap = devm_regmap_init_i2c(client, &ub953_regmap_config);
1289         if (IS_ERR(priv->regmap)) {
1290                 ret = PTR_ERR(priv->regmap);
1291                 dev_err_probe(dev, ret, "Failed to init regmap\n");
1292                 goto err_mutex_destroy;
1293         }
1294
1295         ret = ub953_parse_dt(priv);
1296         if (ret)
1297                 goto err_mutex_destroy;
1298
1299         ret = ub953_hw_init(priv);
1300         if (ret)
1301                 goto err_mutex_destroy;
1302
1303         ret = ub953_gpiochip_probe(priv);
1304         if (ret) {
1305                 dev_err_probe(dev, ret, "Failed to init gpiochip\n");
1306                 goto err_mutex_destroy;
1307         }
1308
1309         ret = ub953_register_clkout(priv);
1310         if (ret) {
1311                 dev_err_probe(dev, ret, "Failed to register clkout\n");
1312                 goto err_gpiochip_remove;
1313         }
1314
1315         ret = ub953_subdev_init(priv);
1316         if (ret)
1317                 goto err_gpiochip_remove;
1318
1319         ret = ub953_add_i2c_adapter(priv);
1320         if (ret) {
1321                 dev_err_probe(dev, ret, "failed to add remote i2c adapter\n");
1322                 goto err_subdev_uninit;
1323         }
1324
1325         return 0;
1326
1327 err_subdev_uninit:
1328         ub953_subdev_uninit(priv);
1329 err_gpiochip_remove:
1330         ub953_gpiochip_remove(priv);
1331 err_mutex_destroy:
1332         mutex_destroy(&priv->reg_lock);
1333
1334         return ret;
1335 }
1336
1337 static void ub953_remove(struct i2c_client *client)
1338 {
1339         struct v4l2_subdev *sd = i2c_get_clientdata(client);
1340         struct ub953_data *priv = sd_to_ub953(sd);
1341
1342         i2c_atr_del_adapter(priv->plat_data->atr, priv->plat_data->port);
1343
1344         ub953_subdev_uninit(priv);
1345
1346         ub953_gpiochip_remove(priv);
1347         mutex_destroy(&priv->reg_lock);
1348 }
1349
1350 static const struct ub953_hw_data ds90ub953_hw = {
1351         .model = "ub953",
1352 };
1353
1354 static const struct ub953_hw_data ds90ub971_hw = {
1355         .model = "ub971",
1356         .is_ub971 = true,
1357 };
1358
1359 static const struct i2c_device_id ub953_id[] = {
1360         { "ds90ub953-q1", (kernel_ulong_t)&ds90ub953_hw },
1361         { "ds90ub971-q1", (kernel_ulong_t)&ds90ub971_hw },
1362         {}
1363 };
1364 MODULE_DEVICE_TABLE(i2c, ub953_id);
1365
1366 static const struct of_device_id ub953_dt_ids[] = {
1367         { .compatible = "ti,ds90ub953-q1", .data = &ds90ub953_hw },
1368         { .compatible = "ti,ds90ub971-q1", .data = &ds90ub971_hw },
1369         {}
1370 };
1371 MODULE_DEVICE_TABLE(of, ub953_dt_ids);
1372
1373 static struct i2c_driver ds90ub953_driver = {
1374         .probe          = ub953_probe,
1375         .remove         = ub953_remove,
1376         .id_table       = ub953_id,
1377         .driver = {
1378                 .name   = "ds90ub953",
1379                 .of_match_table = ub953_dt_ids,
1380         },
1381 };
1382 module_i2c_driver(ds90ub953_driver);
1383
1384 MODULE_LICENSE("GPL");
1385 MODULE_DESCRIPTION("Texas Instruments FPD-Link III/IV CSI-2 Serializers Driver");
1386 MODULE_AUTHOR("Luca Ceresoli <luca@lucaceresoli.net>");
1387 MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>");
1388 MODULE_IMPORT_NS(I2C_ATR);