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