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