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-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_format(state, format->pad, format->stream);
 562         if (!fmt)
 563                 return -EINVAL;
 564
 565         *fmt = format->format;
 566
 567         /* Propagate to source format */
 568         fmt = v4l2_subdev_state_get_opposite_stream_format(state, format->pad,
 569                                                            format->stream);
 570         if (!fmt)
 571                 return -EINVAL;
 572
 573         *fmt = format->format;
 574
 575         return 0;
 576 }
 577
 578 static int ub953_init_state(struct v4l2_subdev *sd,
 579                             struct v4l2_subdev_state *state)
 580 {
 581         struct v4l2_subdev_route routes[] = {
 582                 {
 583                         .sink_pad = UB953_PAD_SINK,
 584                         .sink_stream = 0,
 585                         .source_pad = UB953_PAD_SOURCE,
 586                         .source_stream = 0,
 587                         .flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE,
 588                 },
 589         };
 590
 591         struct v4l2_subdev_krouting routing = {
 592                 .num_routes = ARRAY_SIZE(routes),
 593                 .routes = routes,
 594         };
 595
 596         return _ub953_set_routing(sd, state, &routing);
 597 }
 598
 599 static int ub953_log_status(struct v4l2_subdev *sd)
 600 {
 601         struct ub953_data *priv = sd_to_ub953(sd);
 602         struct device *dev = &priv->client->dev;
 603         u8 v = 0, v1 = 0, v2 = 0;
 604         unsigned int i;
 605         char id[UB953_REG_FPD3_RX_ID_LEN];
 606         u8 gpio_local_data = 0;
 607         u8 gpio_input_ctrl = 0;
 608         u8 gpio_pin_sts = 0;
 609
 610         for (i = 0; i < sizeof(id); i++)
 611                 ub953_read(priv, UB953_REG_FPD3_RX_ID(i), &id[i]);
 612
 613         dev_info(dev, "ID '%.*s'\n", (int)sizeof(id), id);
 614
 615         ub953_read(priv, UB953_REG_GENERAL_STATUS, &v);
 616         dev_info(dev, "GENERAL_STATUS %#02x\n", v);
 617
 618         ub953_read(priv, UB953_REG_CRC_ERR_CNT1, &v1);
 619         ub953_read(priv, UB953_REG_CRC_ERR_CNT2, &v2);
 620         dev_info(dev, "CRC error count %u\n", v1 | (v2 << 8));
 621
 622         ub953_read(priv, UB953_REG_CSI_ERR_CNT, &v);
 623         dev_info(dev, "CSI error count %u\n", v);
 624
 625         ub953_read(priv, UB953_REG_CSI_ERR_STATUS, &v);
 626         dev_info(dev, "CSI_ERR_STATUS %#02x\n", v);
 627
 628         ub953_read(priv, UB953_REG_CSI_ERR_DLANE01, &v);
 629         dev_info(dev, "CSI_ERR_DLANE01 %#02x\n", v);
 630
 631         ub953_read(priv, UB953_REG_CSI_ERR_DLANE23, &v);
 632         dev_info(dev, "CSI_ERR_DLANE23 %#02x\n", v);
 633
 634         ub953_read(priv, UB953_REG_CSI_ERR_CLK_LANE, &v);
 635         dev_info(dev, "CSI_ERR_CLK_LANE %#02x\n", v);
 636
 637         ub953_read(priv, UB953_REG_CSI_PKT_HDR_VC_ID, &v);
 638         dev_info(dev, "CSI packet header VC %u ID %u\n", v >> 6, v & 0x3f);
 639
 640         ub953_read(priv, UB953_REG_PKT_HDR_WC_LSB, &v1);
 641         ub953_read(priv, UB953_REG_PKT_HDR_WC_MSB, &v2);
 642         dev_info(dev, "CSI packet header WC %u\n", (v2 << 8) | v1);
 643
 644         ub953_read(priv, UB953_REG_CSI_ECC, &v);
 645         dev_info(dev, "CSI ECC %#02x\n", v);
 646
 647         ub953_read(priv, UB953_REG_LOCAL_GPIO_DATA, &gpio_local_data);
 648         ub953_read(priv, UB953_REG_GPIO_INPUT_CTRL, &gpio_input_ctrl);
 649         ub953_read(priv, UB953_REG_GPIO_PIN_STS, &gpio_pin_sts);
 650
 651         for (i = 0; i < UB953_NUM_GPIOS; i++) {
 652                 dev_info(dev,
 653                          "GPIO%u: remote: %u is_input: %u is_output: %u val: %u sts: %u\n",
 654                          i,
 655                          !!(gpio_local_data & UB953_REG_LOCAL_GPIO_DATA_GPIO_RMTEN(i)),
 656                          !!(gpio_input_ctrl & UB953_REG_GPIO_INPUT_CTRL_INPUT_EN(i)),
 657                          !!(gpio_input_ctrl & UB953_REG_GPIO_INPUT_CTRL_OUT_EN(i)),
 658                          !!(gpio_local_data & UB953_REG_LOCAL_GPIO_DATA_GPIO_OUT_SRC(i)),
 659                          !!(gpio_pin_sts & UB953_REG_GPIO_PIN_STS_GPIO_STS(i)));
 660         }
 661
 662         return 0;
 663 }
 664
 665 static int ub953_enable_streams(struct v4l2_subdev *sd,
 666                                 struct v4l2_subdev_state *state, u32 pad,
 667                                 u64 streams_mask)
 668 {
 669         struct ub953_data *priv = sd_to_ub953(sd);
 670         u64 sink_streams;
 671         int ret;
 672
 673         sink_streams = v4l2_subdev_state_xlate_streams(state, UB953_PAD_SOURCE,
 674                                                        UB953_PAD_SINK,
 675                                                        &streams_mask);
 676
 677         ret = v4l2_subdev_enable_streams(priv->source_sd, priv->source_sd_pad,
 678                                          sink_streams);
 679         if (ret)
 680                 return ret;
 681
 682         priv->enabled_source_streams |= streams_mask;
 683
 684         return 0;
 685 }
 686
 687 static int ub953_disable_streams(struct v4l2_subdev *sd,
 688                                  struct v4l2_subdev_state *state, u32 pad,
 689                                  u64 streams_mask)
 690 {
 691         struct ub953_data *priv = sd_to_ub953(sd);
 692         u64 sink_streams;
 693         int ret;
 694
 695         sink_streams = v4l2_subdev_state_xlate_streams(state, UB953_PAD_SOURCE,
 696                                                        UB953_PAD_SINK,
 697                                                        &streams_mask);
 698
 699         ret = v4l2_subdev_disable_streams(priv->source_sd, priv->source_sd_pad,
 700                                           sink_streams);
 701         if (ret)
 702                 return ret;
 703
 704         priv->enabled_source_streams &= ~streams_mask;
 705
 706         return 0;
 707 }
 708
 709 static const struct v4l2_subdev_pad_ops ub953_pad_ops = {
 710         .enable_streams = ub953_enable_streams,
 711         .disable_streams = ub953_disable_streams,
 712         .set_routing = ub953_set_routing,
 713         .get_frame_desc = ub953_get_frame_desc,
 714         .get_fmt = v4l2_subdev_get_fmt,
 715         .set_fmt = ub953_set_fmt,
 716 };
 717
 718 static const struct v4l2_subdev_core_ops ub953_subdev_core_ops = {
 719         .log_status = ub953_log_status,
 720         .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
 721         .unsubscribe_event = v4l2_event_subdev_unsubscribe,
 722 };
 723
 724 static const struct v4l2_subdev_ops ub953_subdev_ops = {
 725         .core = &ub953_subdev_core_ops,
 726         .pad = &ub953_pad_ops,
 727 };
 728
 729 static const struct v4l2_subdev_internal_ops ub953_internal_ops = {
 730         .init_state = ub953_init_state,
 731 };
 732
 733 static const struct media_entity_operations ub953_entity_ops = {
 734         .link_validate = v4l2_subdev_link_validate,
 735 };
 736
 737 static int ub953_notify_bound(struct v4l2_async_notifier *notifier,
 738                               struct v4l2_subdev *source_subdev,
 739                               struct v4l2_async_connection *asd)
 740 {
 741         struct ub953_data *priv = sd_to_ub953(notifier->sd);
 742         struct device *dev = &priv->client->dev;
 743         int ret;
 744
 745         ret = media_entity_get_fwnode_pad(&source_subdev->entity,
 746                                           source_subdev->fwnode,
 747                                           MEDIA_PAD_FL_SOURCE);
 748         if (ret < 0) {
 749                 dev_err(dev, "Failed to find pad for %s\n",
 750                         source_subdev->name);
 751                 return ret;
 752         }
 753
 754         priv->source_sd = source_subdev;
 755         priv->source_sd_pad = ret;
 756
 757         ret = media_create_pad_link(&source_subdev->entity, priv->source_sd_pad,
 758                                     &priv->sd.entity, 0,
 759                                     MEDIA_LNK_FL_ENABLED |
 760                                             MEDIA_LNK_FL_IMMUTABLE);
 761         if (ret) {
 762                 dev_err(dev, "Unable to link %s:%u -> %s:0\n",
 763                         source_subdev->name, priv->source_sd_pad,
 764                         priv->sd.name);
 765                 return ret;
 766         }
 767
 768         return 0;
 769 }
 770
 771 static const struct v4l2_async_notifier_operations ub953_notify_ops = {
 772         .bound = ub953_notify_bound,
 773 };
 774
 775 static int ub953_v4l2_notifier_register(struct ub953_data *priv)
 776 {
 777         struct device *dev = &priv->client->dev;
 778         struct v4l2_async_connection *asd;
 779         struct fwnode_handle *ep_fwnode;
 780         int ret;
 781
 782         ep_fwnode = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev),
 783                                                     UB953_PAD_SINK, 0, 0);
 784         if (!ep_fwnode) {
 785                 dev_err(dev, "No graph endpoint\n");
 786                 return -ENODEV;
 787         }
 788
 789         v4l2_async_subdev_nf_init(&priv->notifier, &priv->sd);
 790
 791         asd = v4l2_async_nf_add_fwnode_remote(&priv->notifier, ep_fwnode,
 792                                               struct v4l2_async_connection);
 793
 794         fwnode_handle_put(ep_fwnode);
 795
 796         if (IS_ERR(asd)) {
 797                 dev_err(dev, "Failed to add subdev: %ld", PTR_ERR(asd));
 798                 v4l2_async_nf_cleanup(&priv->notifier);
 799                 return PTR_ERR(asd);
 800         }
 801
 802         priv->notifier.ops = &ub953_notify_ops;
 803
 804         ret = v4l2_async_nf_register(&priv->notifier);
 805         if (ret) {
 806                 dev_err(dev, "Failed to register subdev_notifier");
 807                 v4l2_async_nf_cleanup(&priv->notifier);
 808                 return ret;
 809         }
 810
 811         return 0;
 812 }
 813
 814 static void ub953_v4l2_notifier_unregister(struct ub953_data *priv)
 815 {
 816         v4l2_async_nf_unregister(&priv->notifier);
 817         v4l2_async_nf_cleanup(&priv->notifier);
 818 }
 819
 820 /*
 821  * Probing
 822  */
 823
 824 static int ub953_i2c_master_init(struct ub953_data *priv)
 825 {
 826         /* i2c fast mode */
 827         u32 ref = 26250000;
 828         u32 scl_high = 915; /* ns */
 829         u32 scl_low = 1641; /* ns */
 830         int ret;
 831
 832         scl_high = div64_u64((u64)scl_high * ref, 1000000000) - 5;
 833         scl_low = div64_u64((u64)scl_low * ref, 1000000000) - 5;
 834
 835         ret = ub953_write(priv, UB953_REG_SCL_HIGH_TIME, scl_high);
 836         if (ret)
 837                 return ret;
 838
 839         ret = ub953_write(priv, UB953_REG_SCL_LOW_TIME, scl_low);
 840         if (ret)
 841                 return ret;
 842
 843         return 0;
 844 }
 845
 846 static u64 ub953_get_fc_rate(struct ub953_data *priv)
 847 {
 848         switch (priv->mode) {
 849         case UB953_MODE_SYNC:
 850                 if (priv->hw_data->is_ub971)
 851                         return priv->plat_data->bc_rate * 160ull;
 852                 else
 853                         return priv->plat_data->bc_rate / 2 * 160ull;
 854
 855         case UB953_MODE_NONSYNC_EXT:
 856                 /* CLKIN_DIV = 1 always */
 857                 return clk_get_rate(priv->clkin) * 80ull;
 858
 859         default:
 860                 /* Not supported */
 861                 return 0;
 862         }
 863 }
 864
 865 static unsigned long ub953_calc_clkout_ub953(struct ub953_data *priv,
 866                                              unsigned long target, u64 fc,
 867                                              u8 *hs_div, u8 *m, u8 *n)
 868 {
 869         /*
 870          * We always use 4 as a pre-divider (HS_CLK_DIV = 2).
 871          *
 872          * According to the datasheet:
 873          * - "HS_CLK_DIV typically should be set to either 16, 8, or 4 (default)."
 874          * - "if it is not possible to have an integer ratio of N/M, it is best to
 875          *    select a smaller value for HS_CLK_DIV.
 876          *
 877          * For above reasons the default HS_CLK_DIV seems the best in the average
 878          * case. Use always that value to keep the code simple.
 879          */
 880         static const unsigned long hs_clk_div = 4;
 881
 882         u64 fc_divided;
 883         unsigned long mul, div;
 884         unsigned long res;
 885
 886         /* clkout = fc / hs_clk_div * m / n */
 887
 888         fc_divided = div_u64(fc, hs_clk_div);
 889
 890         rational_best_approximation(target, fc_divided, (1 << 5) - 1,
 891                                     (1 << 8) - 1, &mul, &div);
 892
 893         res = div_u64(fc_divided * mul, div);
 894
 895         *hs_div = hs_clk_div;
 896         *m = mul;
 897         *n = div;
 898
 899         return res;
 900 }
 901
 902 static unsigned long ub953_calc_clkout_ub971(struct ub953_data *priv,
 903                                              unsigned long target, u64 fc,
 904                                              u8 *m, u8 *n)
 905 {
 906         u64 fc_divided;
 907         unsigned long mul, div;
 908         unsigned long res;
 909
 910         /* clkout = fc * m / (8 * n) */
 911
 912         fc_divided = div_u64(fc, 8);
 913
 914         rational_best_approximation(target, fc_divided, (1 << 5) - 1,
 915                                     (1 << 8) - 1, &mul, &div);
 916
 917         res = div_u64(fc_divided * mul, div);
 918
 919         *m = mul;
 920         *n = div;
 921
 922         return res;
 923 }
 924
 925 static void ub953_calc_clkout_params(struct ub953_data *priv,
 926                                      unsigned long target_rate,
 927                                      struct ub953_clkout_data *clkout_data)
 928 {
 929         struct device *dev = &priv->client->dev;
 930         unsigned long clkout_rate;
 931         u64 fc_rate;
 932
 933         fc_rate = ub953_get_fc_rate(priv);
 934
 935         if (priv->hw_data->is_ub971) {
 936                 u8 m, n;
 937
 938                 clkout_rate = ub953_calc_clkout_ub971(priv, target_rate,
 939                                                       fc_rate, &m, &n);
 940
 941                 clkout_data->m = m;
 942                 clkout_data->n = n;
 943
 944                 dev_dbg(dev, "%s %llu * %u / (8 * %u) = %lu (requested %lu)",
 945                         __func__, fc_rate, m, n, clkout_rate, target_rate);
 946         } else {
 947                 u8 hs_div, m, n;
 948
 949                 clkout_rate = ub953_calc_clkout_ub953(priv, target_rate,
 950                                                       fc_rate, &hs_div, &m, &n);
 951
 952                 clkout_data->hs_div = hs_div;
 953                 clkout_data->m = m;
 954                 clkout_data->n = n;
 955
 956                 dev_dbg(dev, "%s %llu / %u * %u / %u = %lu (requested %lu)",
 957                         __func__, fc_rate, hs_div, m, n, clkout_rate,
 958                         target_rate);
 959         }
 960
 961         clkout_data->rate = clkout_rate;
 962 }
 963
 964 static void ub953_write_clkout_regs(struct ub953_data *priv,
 965                                     const struct ub953_clkout_data *clkout_data)
 966 {
 967         u8 clkout_ctrl0, clkout_ctrl1;
 968
 969         if (priv->hw_data->is_ub971)
 970                 clkout_ctrl0 = clkout_data->m;
 971         else
 972                 clkout_ctrl0 = (__ffs(clkout_data->hs_div) << 5) |
 973                                clkout_data->m;
 974
 975         clkout_ctrl1 = clkout_data->n;
 976
 977         ub953_write(priv, UB953_REG_CLKOUT_CTRL0, clkout_ctrl0);
 978         ub953_write(priv, UB953_REG_CLKOUT_CTRL1, clkout_ctrl1);
 979 }
 980
 981 static unsigned long ub953_clkout_recalc_rate(struct clk_hw *hw,
 982                                               unsigned long parent_rate)
 983 {
 984         struct ub953_data *priv = container_of(hw, struct ub953_data, clkout_clk_hw);
 985         struct device *dev = &priv->client->dev;
 986         u8 ctrl0, ctrl1;
 987         u32 mul, div;
 988         u64 fc_rate;
 989         u32 hs_clk_div;
 990         u64 rate;
 991         int ret;
 992
 993         ret = ub953_read(priv, UB953_REG_CLKOUT_CTRL0, &ctrl0);
 994         if (ret) {
 995                 dev_err(dev, "Failed to read CLKOUT_CTRL0: %d\n", ret);
 996                 return 0;
 997         }
 998
 999         ret = ub953_read(priv, UB953_REG_CLKOUT_CTRL1, &ctrl1);
1000         if (ret) {
1001                 dev_err(dev, "Failed to read CLKOUT_CTRL1: %d\n", ret);
1002                 return 0;
1003         }
1004
1005         fc_rate = ub953_get_fc_rate(priv);
1006
1007         if (priv->hw_data->is_ub971) {
1008                 mul = ctrl0 & 0x1f;
1009                 div = ctrl1;
1010
1011                 if (div == 0)
1012                         return 0;
1013
1014                 rate = div_u64(fc_rate * mul, 8 * div);
1015
1016                 dev_dbg(dev, "clkout: fc rate %llu, mul %u, div %u = %llu\n",
1017                         fc_rate, mul, div, rate);
1018         } else {
1019                 mul = ctrl0 & 0x1f;
1020                 hs_clk_div = 1 << (ctrl0 >> 5);
1021                 div = ctrl1;
1022
1023                 if (div == 0)
1024                         return 0;
1025
1026                 rate = div_u64(div_u64(fc_rate, hs_clk_div) * mul, div);
1027
1028                 dev_dbg(dev,
1029                         "clkout: fc rate %llu, hs_clk_div %u, mul %u, div %u = %llu\n",
1030                         fc_rate, hs_clk_div, mul, div, rate);
1031         }
1032
1033         return rate;
1034 }
1035
1036 static long ub953_clkout_round_rate(struct clk_hw *hw, unsigned long rate,
1037                                     unsigned long *parent_rate)
1038 {
1039         struct ub953_data *priv = container_of(hw, struct ub953_data, clkout_clk_hw);
1040         struct ub953_clkout_data clkout_data;
1041
1042         ub953_calc_clkout_params(priv, rate, &clkout_data);
1043
1044         return clkout_data.rate;
1045 }
1046
1047 static int ub953_clkout_set_rate(struct clk_hw *hw, unsigned long rate,
1048                                  unsigned long parent_rate)
1049 {
1050         struct ub953_data *priv = container_of(hw, struct ub953_data, clkout_clk_hw);
1051         struct ub953_clkout_data clkout_data;
1052
1053         ub953_calc_clkout_params(priv, rate, &clkout_data);
1054
1055         dev_dbg(&priv->client->dev, "%s %lu (requested %lu)\n", __func__,
1056                 clkout_data.rate, rate);
1057
1058         ub953_write_clkout_regs(priv, &clkout_data);
1059
1060         return 0;
1061 }
1062
1063 static const struct clk_ops ub953_clkout_ops = {
1064         .recalc_rate    = ub953_clkout_recalc_rate,
1065         .round_rate     = ub953_clkout_round_rate,
1066         .set_rate       = ub953_clkout_set_rate,
1067 };
1068
1069 static int ub953_register_clkout(struct ub953_data *priv)
1070 {
1071         struct device *dev = &priv->client->dev;
1072         const struct clk_init_data init = {
1073                 .name = kasprintf(GFP_KERNEL, "ds90%s.%s.clk_out",
1074                                   priv->hw_data->model, dev_name(dev)),
1075                 .ops = &ub953_clkout_ops,
1076         };
1077         struct ub953_clkout_data clkout_data;
1078         int ret;
1079
1080         if (!init.name)
1081                 return -ENOMEM;
1082
1083         /* Initialize clkout to 25MHz by default */
1084         ub953_calc_clkout_params(priv, UB953_DEFAULT_CLKOUT_RATE, &clkout_data);
1085         ub953_write_clkout_regs(priv, &clkout_data);
1086
1087         priv->clkout_clk_hw.init = &init;
1088
1089         ret = devm_clk_hw_register(dev, &priv->clkout_clk_hw);
1090         kfree(init.name);
1091         if (ret)
1092                 return dev_err_probe(dev, ret, "Cannot register clock HW\n");
1093
1094         ret = devm_of_clk_add_hw_provider(dev, of_clk_hw_simple_get,
1095                                           &priv->clkout_clk_hw);
1096         if (ret)
1097                 return dev_err_probe(dev, ret,
1098                                      "Cannot add OF clock provider\n");
1099
1100         return 0;
1101 }
1102
1103 static int ub953_add_i2c_adapter(struct ub953_data *priv)
1104 {
1105         struct device *dev = &priv->client->dev;
1106         struct fwnode_handle *i2c_handle;
1107         int ret;
1108
1109         i2c_handle = device_get_named_child_node(dev, "i2c");
1110         if (!i2c_handle)
1111                 return 0;
1112
1113         ret = i2c_atr_add_adapter(priv->plat_data->atr, priv->plat_data->port,
1114                                   dev, i2c_handle);
1115
1116         fwnode_handle_put(i2c_handle);
1117
1118         if (ret)
1119                 return ret;
1120
1121         return 0;
1122 }
1123
1124 static const struct regmap_config ub953_regmap_config = {
1125         .name = "ds90ub953",
1126         .reg_bits = 8,
1127         .val_bits = 8,
1128         .reg_format_endian = REGMAP_ENDIAN_DEFAULT,
1129         .val_format_endian = REGMAP_ENDIAN_DEFAULT,
1130 };
1131
1132 static int ub953_parse_dt(struct ub953_data *priv)
1133 {
1134         struct device *dev = &priv->client->dev;
1135         struct v4l2_fwnode_endpoint vep = {
1136                 .bus_type = V4L2_MBUS_CSI2_DPHY,
1137         };
1138         struct fwnode_handle *ep_fwnode;
1139         unsigned char nlanes;
1140         int ret;
1141
1142         ep_fwnode = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev),
1143                                                     UB953_PAD_SINK, 0, 0);
1144         if (!ep_fwnode)
1145                 return dev_err_probe(dev, -ENOENT, "no endpoint found\n");
1146
1147         ret = v4l2_fwnode_endpoint_parse(ep_fwnode, &vep);
1148
1149         fwnode_handle_put(ep_fwnode);
1150
1151         if (ret)
1152                 return dev_err_probe(dev, ret,
1153                                      "failed to parse sink endpoint data\n");
1154
1155         nlanes = vep.bus.mipi_csi2.num_data_lanes;
1156         if (nlanes != 1 && nlanes != 2 && nlanes != 4)
1157                 return dev_err_probe(dev, -EINVAL,
1158                                      "bad number of data-lanes: %u\n", nlanes);
1159
1160         priv->num_data_lanes = nlanes;
1161
1162         priv->non_continous_clk = vep.bus.mipi_csi2.flags &
1163                                   V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK;
1164
1165         return 0;
1166 }
1167
1168 static int ub953_hw_init(struct ub953_data *priv)
1169 {
1170         struct device *dev = &priv->client->dev;
1171         bool mode_override;
1172         int ret;
1173         u8 v;
1174
1175         ret = ub953_read(priv, UB953_REG_MODE_SEL, &v);
1176         if (ret)
1177                 return ret;
1178
1179         if (!(v & UB953_REG_MODE_SEL_MODE_DONE))
1180                 return dev_err_probe(dev, -EIO, "Mode value not stabilized\n");
1181
1182         mode_override = v & UB953_REG_MODE_SEL_MODE_OVERRIDE;
1183
1184         switch (v & UB953_REG_MODE_SEL_MODE_MASK) {
1185         case 0:
1186                 priv->mode = UB953_MODE_SYNC;
1187                 break;
1188         case 2:
1189                 priv->mode = UB953_MODE_NONSYNC_EXT;
1190                 break;
1191         case 3:
1192                 priv->mode = UB953_MODE_NONSYNC_INT;
1193                 break;
1194         case 5:
1195                 priv->mode = UB953_MODE_DVP;
1196                 break;
1197         default:
1198                 return dev_err_probe(dev, -EIO,
1199                                      "Invalid mode in mode register\n");
1200         }
1201
1202         dev_dbg(dev, "mode from %s: %#x\n", mode_override ? "reg" : "strap",
1203                 priv->mode);
1204
1205         if (priv->mode != UB953_MODE_SYNC &&
1206             priv->mode != UB953_MODE_NONSYNC_EXT)
1207                 return dev_err_probe(dev, -ENODEV,
1208                                      "Unsupported mode selected: %u\n",
1209                                      priv->mode);
1210
1211         if (priv->mode == UB953_MODE_NONSYNC_EXT && !priv->clkin)
1212                 return dev_err_probe(dev, -EINVAL,
1213                                      "clkin required for non-sync ext mode\n");
1214
1215         ret = ub953_read(priv, UB953_REG_REV_MASK_ID, &v);
1216         if (ret)
1217                 return dev_err_probe(dev, ret, "Failed to read revision");
1218
1219         dev_info(dev, "Found %s rev/mask %#04x\n", priv->hw_data->model, v);
1220
1221         ret = ub953_read(priv, UB953_REG_GENERAL_CFG, &v);
1222         if (ret)
1223                 return ret;
1224
1225         dev_dbg(dev, "i2c strap setting %s V\n",
1226                 (v & UB953_REG_GENERAL_CFG_I2C_STRAP_MODE) ? "1.8" : "3.3");
1227
1228         ret = ub953_i2c_master_init(priv);
1229         if (ret)
1230                 return dev_err_probe(dev, ret, "i2c init failed\n");
1231
1232         ub953_write(priv, UB953_REG_GENERAL_CFG,
1233                     (priv->non_continous_clk ? 0 : UB953_REG_GENERAL_CFG_CONT_CLK) |
1234                     ((priv->num_data_lanes - 1) << UB953_REG_GENERAL_CFG_CSI_LANE_SEL_SHIFT) |
1235                     UB953_REG_GENERAL_CFG_CRC_TX_GEN_ENABLE);
1236
1237         return 0;
1238 }
1239
1240 static int ub953_subdev_init(struct ub953_data *priv)
1241 {
1242         struct device *dev = &priv->client->dev;
1243         int ret;
1244
1245         v4l2_i2c_subdev_init(&priv->sd, priv->client, &ub953_subdev_ops);
1246         priv->sd.internal_ops = &ub953_internal_ops;
1247
1248         priv->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
1249                           V4L2_SUBDEV_FL_HAS_EVENTS | V4L2_SUBDEV_FL_STREAMS;
1250         priv->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
1251         priv->sd.entity.ops = &ub953_entity_ops;
1252
1253         priv->pads[0].flags = MEDIA_PAD_FL_SINK;
1254         priv->pads[1].flags = MEDIA_PAD_FL_SOURCE;
1255
1256         ret = media_entity_pads_init(&priv->sd.entity, 2, priv->pads);
1257         if (ret)
1258                 return dev_err_probe(dev, ret, "Failed to init pads\n");
1259
1260         ret = v4l2_subdev_init_finalize(&priv->sd);
1261         if (ret)
1262                 goto err_entity_cleanup;
1263
1264         ret = ub953_v4l2_notifier_register(priv);
1265         if (ret) {
1266                 dev_err_probe(dev, ret,
1267                               "v4l2 subdev notifier register failed\n");
1268                 goto err_free_state;
1269         }
1270
1271         ret = v4l2_async_register_subdev(&priv->sd);
1272         if (ret) {
1273                 dev_err_probe(dev, ret, "v4l2_async_register_subdev error\n");
1274                 goto err_unreg_notif;
1275         }
1276
1277         return 0;
1278
1279 err_unreg_notif:
1280         ub953_v4l2_notifier_unregister(priv);
1281 err_free_state:
1282         v4l2_subdev_cleanup(&priv->sd);
1283 err_entity_cleanup:
1284         media_entity_cleanup(&priv->sd.entity);
1285
1286         return ret;
1287 }
1288
1289 static void ub953_subdev_uninit(struct ub953_data *priv)
1290 {
1291         v4l2_async_unregister_subdev(&priv->sd);
1292         ub953_v4l2_notifier_unregister(priv);
1293         v4l2_subdev_cleanup(&priv->sd);
1294         fwnode_handle_put(priv->sd.fwnode);
1295         media_entity_cleanup(&priv->sd.entity);
1296 }
1297
1298 static int ub953_probe(struct i2c_client *client)
1299 {
1300         struct device *dev = &client->dev;
1301         struct ub953_data *priv;
1302         int ret;
1303
1304         priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
1305         if (!priv)
1306                 return -ENOMEM;
1307
1308         priv->client = client;
1309
1310         priv->hw_data = device_get_match_data(dev);
1311
1312         priv->plat_data = dev_get_platdata(&client->dev);
1313         if (!priv->plat_data)
1314                 return dev_err_probe(dev, -ENODEV, "Platform data missing\n");
1315
1316         mutex_init(&priv->reg_lock);
1317
1318         /*
1319          * Initialize to invalid values so that the first reg writes will
1320          * configure the target.
1321          */
1322         priv->current_indirect_target = 0xff;
1323
1324         priv->regmap = devm_regmap_init_i2c(client, &ub953_regmap_config);
1325         if (IS_ERR(priv->regmap)) {
1326                 ret = PTR_ERR(priv->regmap);
1327                 dev_err_probe(dev, ret, "Failed to init regmap\n");
1328                 goto err_mutex_destroy;
1329         }
1330
1331         priv->clkin = devm_clk_get_optional(dev, "clkin");
1332         if (IS_ERR(priv->clkin)) {
1333                 ret = PTR_ERR(priv->clkin);
1334                 dev_err_probe(dev, ret, "failed to parse 'clkin'\n");
1335                 goto err_mutex_destroy;
1336         }
1337
1338         ret = ub953_parse_dt(priv);
1339         if (ret)
1340                 goto err_mutex_destroy;
1341
1342         ret = ub953_hw_init(priv);
1343         if (ret)
1344                 goto err_mutex_destroy;
1345
1346         ret = ub953_gpiochip_probe(priv);
1347         if (ret) {
1348                 dev_err_probe(dev, ret, "Failed to init gpiochip\n");
1349                 goto err_mutex_destroy;
1350         }
1351
1352         ret = ub953_register_clkout(priv);
1353         if (ret) {
1354                 dev_err_probe(dev, ret, "Failed to register clkout\n");
1355                 goto err_gpiochip_remove;
1356         }
1357
1358         ret = ub953_subdev_init(priv);
1359         if (ret)
1360                 goto err_gpiochip_remove;
1361
1362         ret = ub953_add_i2c_adapter(priv);
1363         if (ret) {
1364                 dev_err_probe(dev, ret, "failed to add remote i2c adapter\n");
1365                 goto err_subdev_uninit;
1366         }
1367
1368         return 0;
1369
1370 err_subdev_uninit:
1371         ub953_subdev_uninit(priv);
1372 err_gpiochip_remove:
1373         ub953_gpiochip_remove(priv);
1374 err_mutex_destroy:
1375         mutex_destroy(&priv->reg_lock);
1376
1377         return ret;
1378 }
1379
1380 static void ub953_remove(struct i2c_client *client)
1381 {
1382         struct v4l2_subdev *sd = i2c_get_clientdata(client);
1383         struct ub953_data *priv = sd_to_ub953(sd);
1384
1385         i2c_atr_del_adapter(priv->plat_data->atr, priv->plat_data->port);
1386
1387         ub953_subdev_uninit(priv);
1388
1389         ub953_gpiochip_remove(priv);
1390         mutex_destroy(&priv->reg_lock);
1391 }
1392
1393 static const struct ub953_hw_data ds90ub953_hw = {
1394         .model = "ub953",
1395 };
1396
1397 static const struct ub953_hw_data ds90ub971_hw = {
1398         .model = "ub971",
1399         .is_ub971 = true,
1400 };
1401
1402 static const struct i2c_device_id ub953_id[] = {
1403         { "ds90ub953-q1", (kernel_ulong_t)&ds90ub953_hw },
1404         { "ds90ub971-q1", (kernel_ulong_t)&ds90ub971_hw },
1405         {}
1406 };
1407 MODULE_DEVICE_TABLE(i2c, ub953_id);
1408
1409 static const struct of_device_id ub953_dt_ids[] = {
1410         { .compatible = "ti,ds90ub953-q1", .data = &ds90ub953_hw },
1411         { .compatible = "ti,ds90ub971-q1", .data = &ds90ub971_hw },
1412         {}
1413 };
1414 MODULE_DEVICE_TABLE(of, ub953_dt_ids);
1415
1416 static struct i2c_driver ds90ub953_driver = {
1417         .probe          = ub953_probe,
1418         .remove         = ub953_remove,
1419         .id_table       = ub953_id,
1420         .driver = {
1421                 .name   = "ds90ub953",
1422                 .of_match_table = ub953_dt_ids,
1423         },
1424 };
1425 module_i2c_driver(ds90ub953_driver);
1426
1427 MODULE_LICENSE("GPL");
1428 MODULE_DESCRIPTION("Texas Instruments FPD-Link III/IV CSI-2 Serializers Driver");
1429 MODULE_AUTHOR("Luca Ceresoli <luca@lucaceresoli.net>");
1430 MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>");
1431 MODULE_IMPORT_NS(I2C_ATR);