drivers/media/i2c/tc358746.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * TC358746 - Parallel <-> CSI-2 Bridge
   4  *
   5  * Copyright 2022 Marco Felsch <kernel@pengutronix.de>
   6  *
   7  * Notes:
   8  *  - Currently only 'Parallel-in -> CSI-out' mode is supported!
   9  */
  10
  11 #include <linux/bitfield.h>
  12 #include <linux/clk.h>
  13 #include <linux/clk-provider.h>
  14 #include <linux/delay.h>
  15 #include <linux/i2c.h>
  16 #include <linux/interrupt.h>
  17 #include <linux/kernel.h>
  18 #include <linux/module.h>
  19 #include <linux/phy/phy-mipi-dphy.h>
  20 #include <linux/property.h>
  21 #include <linux/pm_runtime.h>
  22 #include <linux/regmap.h>
  23 #include <linux/units.h>
  24 #include <media/v4l2-ctrls.h>
  25 #include <media/v4l2-device.h>
  26 #include <media/v4l2-fwnode.h>
  27 #include <media/v4l2-mc.h>
  28
  29 /* 16-bit registers */
  30 #define CHIPID_REG                      0x0000
  31 #define         CHIPID                  GENMASK(15, 8)
  32
  33 #define SYSCTL_REG                      0x0002
  34 #define         SRESET                  BIT(0)
  35
  36 #define CONFCTL_REG                     0x0004
  37 #define         PDATAF_MASK             GENMASK(9, 8)
  38 #define         PDATAF_MODE0            0
  39 #define         PDATAF_MODE1            1
  40 #define         PDATAF_MODE2            2
  41 #define         PDATAF(val)             FIELD_PREP(PDATAF_MASK, (val))
  42 #define         PPEN                    BIT(6)
  43 #define         DATALANE_MASK           GENMASK(1, 0)
  44
  45 #define FIFOCTL_REG                     0x0006
  46 #define DATAFMT_REG                     0x0008
  47 #define         PDFMT(val)              FIELD_PREP(GENMASK(7, 4), (val))
  48
  49 #define MCLKCTL_REG                     0x000c
  50 #define         MCLK_HIGH_MASK          GENMASK(15, 8)
  51 #define         MCLK_LOW_MASK           GENMASK(7, 0)
  52 #define         MCLK_HIGH(val)          FIELD_PREP(MCLK_HIGH_MASK, (val))
  53 #define         MCLK_LOW(val)           FIELD_PREP(MCLK_LOW_MASK, (val))
  54
  55 #define PLLCTL0_REG                     0x0016
  56 #define         PLL_PRD_MASK            GENMASK(15, 12)
  57 #define         PLL_PRD(val)            FIELD_PREP(PLL_PRD_MASK, (val))
  58 #define         PLL_FBD_MASK            GENMASK(8, 0)
  59 #define         PLL_FBD(val)            FIELD_PREP(PLL_FBD_MASK, (val))
  60
  61 #define PLLCTL1_REG                     0x0018
  62 #define         PLL_FRS_MASK            GENMASK(11, 10)
  63 #define         PLL_FRS(val)            FIELD_PREP(PLL_FRS_MASK, (val))
  64 #define         CKEN                    BIT(4)
  65 #define         RESETB                  BIT(1)
  66 #define         PLL_EN                  BIT(0)
  67
  68 #define CLKCTL_REG                      0x0020
  69 #define         MCLKDIV_MASK            GENMASK(3, 2)
  70 #define         MCLKDIV(val)            FIELD_PREP(MCLKDIV_MASK, (val))
  71 #define         MCLKDIV_8               0
  72 #define         MCLKDIV_4               1
  73 #define         MCLKDIV_2               2
  74
  75 #define WORDCNT_REG                     0x0022
  76 #define PP_MISC_REG                     0x0032
  77 #define         FRMSTOP                 BIT(15)
  78 #define         RSTPTR                  BIT(14)
  79
  80 /* 32-bit registers */
  81 #define CLW_DPHYCONTTX_REG              0x0100
  82 #define CLW_CNTRL_REG                   0x0140
  83 #define D0W_CNTRL_REG                   0x0144
  84 #define         LANEDISABLE             BIT(0)
  85
  86 #define STARTCNTRL_REG                  0x0204
  87 #define         START                   BIT(0)
  88
  89 #define LINEINITCNT_REG                 0x0210
  90 #define LPTXTIMECNT_REG                 0x0214
  91 #define TCLK_HEADERCNT_REG              0x0218
  92 #define         TCLK_ZEROCNT(val)       FIELD_PREP(GENMASK(15, 8), (val))
  93 #define         TCLK_PREPARECNT(val)    FIELD_PREP(GENMASK(6, 0), (val))
  94
  95 #define TCLK_TRAILCNT_REG               0x021C
  96 #define THS_HEADERCNT_REG               0x0220
  97 #define         THS_ZEROCNT(val)        FIELD_PREP(GENMASK(14, 8), (val))
  98 #define         THS_PREPARECNT(val)     FIELD_PREP(GENMASK(6, 0), (val))
  99
 100 #define TWAKEUP_REG                     0x0224
 101 #define TCLK_POSTCNT_REG                0x0228
 102 #define THS_TRAILCNT_REG                0x022C
 103 #define HSTXVREGEN_REG                  0x0234
 104 #define TXOPTIONCNTRL_REG               0x0238
 105 #define CSI_CONTROL_REG                 0x040C
 106 #define         CSI_MODE                BIT(15)
 107 #define         TXHSMD                  BIT(7)
 108 #define         NOL(val)                FIELD_PREP(GENMASK(2, 1), (val))
 109
 110 #define CSI_CONFW_REG                   0x0500
 111 #define         MODE(val)               FIELD_PREP(GENMASK(31, 29), (val))
 112 #define         MODE_SET                0x5
 113 #define         ADDRESS(val)            FIELD_PREP(GENMASK(28, 24), (val))
 114 #define         CSI_CONTROL_ADDRESS     0x3
 115 #define         DATA(val)               FIELD_PREP(GENMASK(15, 0), (val))
 116
 117 #define CSI_START_REG                   0x0518
 118 #define         STRT                    BIT(0)
 119
 120 static const struct v4l2_mbus_framefmt tc358746_def_fmt = {
 121         .width          = 640,
 122         .height         = 480,
 123         .code           = MEDIA_BUS_FMT_UYVY8_2X8,
 124         .field          = V4L2_FIELD_NONE,
 125         .colorspace     = V4L2_COLORSPACE_DEFAULT,
 126         .ycbcr_enc      = V4L2_YCBCR_ENC_DEFAULT,
 127         .quantization   = V4L2_QUANTIZATION_DEFAULT,
 128         .xfer_func      = V4L2_XFER_FUNC_DEFAULT,
 129 };
 130
 131 static const char * const tc358746_supplies[] = {
 132         "vddc", "vddio", "vddmipi"
 133 };
 134
 135 enum {
 136         TC358746_SINK,
 137         TC358746_SOURCE,
 138         TC358746_NR_PADS
 139 };
 140
 141 struct tc358746 {
 142         struct v4l2_subdev              sd;
 143         struct media_pad                pads[TC358746_NR_PADS];
 144         struct v4l2_async_notifier      notifier;
 145         struct v4l2_fwnode_endpoint     csi_vep;
 146
 147         struct v4l2_ctrl_handler        ctrl_hdl;
 148
 149         struct regmap                   *regmap;
 150         struct clk                      *refclk;
 151         struct gpio_desc                *reset_gpio;
 152         struct regulator_bulk_data      supplies[ARRAY_SIZE(tc358746_supplies)];
 153
 154         struct clk_hw                   mclk_hw;
 155         unsigned long                   mclk_rate;
 156         u8                              mclk_prediv;
 157         u16                             mclk_postdiv;
 158
 159         unsigned long                   pll_rate;
 160         u8                              pll_post_div;
 161         u16                             pll_pre_div;
 162         u16                             pll_mul;
 163
 164 #define TC358746_VB_MAX_SIZE            (511 * 32)
 165 #define TC358746_VB_DEFAULT_SIZE          (1 * 32)
 166         unsigned int                    vb_size; /* Video buffer size in bits */
 167
 168         struct phy_configure_opts_mipi_dphy dphy_cfg;
 169 };
 170
 171 static inline struct tc358746 *to_tc358746(struct v4l2_subdev *sd)
 172 {
 173         return container_of(sd, struct tc358746, sd);
 174 }
 175
 176 static inline struct tc358746 *clk_hw_to_tc358746(struct clk_hw *hw)
 177 {
 178         return container_of(hw, struct tc358746, mclk_hw);
 179 }
 180
 181 struct tc358746_format {
 182         u32             code;
 183         bool            csi_format;
 184         unsigned char   bus_width;
 185         unsigned char   bpp;
 186         /* Register values */
 187         u8              pdformat; /* Peripheral Data Format */
 188         u8              pdataf;   /* Parallel Data Format Option */
 189 };
 190
 191 enum {
 192         PDFORMAT_RAW8 = 0,
 193         PDFORMAT_RAW10,
 194         PDFORMAT_RAW12,
 195         PDFORMAT_RGB888,
 196         PDFORMAT_RGB666,
 197         PDFORMAT_RGB565,
 198         PDFORMAT_YUV422_8BIT,
 199         /* RESERVED = 7 */
 200         PDFORMAT_RAW14 = 8,
 201         PDFORMAT_YUV422_10BIT,
 202         PDFORMAT_YUV444,
 203 };
 204
 205 /* Check tc358746_src_mbus_code() if you add new formats */
 206 static const struct tc358746_format tc358746_formats[] = {
 207         {
 208                 .code = MEDIA_BUS_FMT_UYVY8_2X8,
 209                 .bus_width = 8,
 210                 .bpp = 16,
 211                 .pdformat = PDFORMAT_YUV422_8BIT,
 212                 .pdataf = PDATAF_MODE0,
 213         }, {
 214                 .code = MEDIA_BUS_FMT_UYVY8_1X16,
 215                 .csi_format = true,
 216                 .bus_width = 16,
 217                 .bpp = 16,
 218                 .pdformat = PDFORMAT_YUV422_8BIT,
 219                 .pdataf = PDATAF_MODE1,
 220         }, {
 221                 .code = MEDIA_BUS_FMT_YUYV8_1X16,
 222                 .csi_format = true,
 223                 .bus_width = 16,
 224                 .bpp = 16,
 225                 .pdformat = PDFORMAT_YUV422_8BIT,
 226                 .pdataf = PDATAF_MODE2,
 227         }, {
 228                 .code = MEDIA_BUS_FMT_UYVY10_2X10,
 229                 .bus_width = 10,
 230                 .bpp = 20,
 231                 .pdformat = PDFORMAT_YUV422_10BIT,
 232                 .pdataf = PDATAF_MODE0, /* don't care */
 233         }
 234 };
 235
 236 /* Get n-th format for pad */
 237 static const struct tc358746_format *
 238 tc358746_get_format_by_idx(unsigned int pad, unsigned int index)
 239 {
 240         unsigned int idx = 0;
 241         unsigned int i;
 242
 243         for (i = 0; i < ARRAY_SIZE(tc358746_formats); i++) {
 244                 const struct tc358746_format *fmt = &tc358746_formats[i];
 245
 246                 if ((pad == TC358746_SOURCE && fmt->csi_format) ||
 247                     (pad == TC358746_SINK)) {
 248                         if (idx == index)
 249                                 return fmt;
 250                         idx++;
 251                 }
 252         }
 253
 254         return ERR_PTR(-EINVAL);
 255 }
 256
 257 static const struct tc358746_format *
 258 tc358746_get_format_by_code(unsigned int pad, u32 code)
 259 {
 260         unsigned int i;
 261
 262         for (i = 0; i < ARRAY_SIZE(tc358746_formats); i++) {
 263                 const struct tc358746_format *fmt = &tc358746_formats[i];
 264
 265                 if (pad == TC358746_SINK && fmt->code == code)
 266                         return fmt;
 267
 268                 if (pad == TC358746_SOURCE && !fmt->csi_format)
 269                         continue;
 270
 271                 if (fmt->code == code)
 272                         return fmt;
 273         }
 274
 275         return ERR_PTR(-EINVAL);
 276 }
 277
 278 static u32 tc358746_src_mbus_code(u32 code)
 279 {
 280         switch (code) {
 281         case MEDIA_BUS_FMT_UYVY8_2X8:
 282                 return MEDIA_BUS_FMT_UYVY8_1X16;
 283         case MEDIA_BUS_FMT_UYVY10_2X10:
 284                 return MEDIA_BUS_FMT_UYVY10_1X20;
 285         default:
 286                 return code;
 287         }
 288 }
 289
 290 static bool tc358746_valid_reg(struct device *dev, unsigned int reg)
 291 {
 292         switch (reg) {
 293         case CHIPID_REG ... CSI_START_REG:
 294                 return true;
 295         default:
 296                 return false;
 297         }
 298 }
 299
 300 static const struct regmap_config tc358746_regmap_config = {
 301         .name = "tc358746",
 302         .reg_bits = 16,
 303         .val_bits = 16,
 304         .max_register = CSI_START_REG,
 305         .writeable_reg = tc358746_valid_reg,
 306         .readable_reg = tc358746_valid_reg,
 307         .reg_format_endian = REGMAP_ENDIAN_BIG,
 308         .val_format_endian = REGMAP_ENDIAN_BIG,
 309 };
 310
 311 static int tc358746_write(struct tc358746 *tc358746, u32 reg, u32 val)
 312 {
 313         size_t count;
 314         int err;
 315
 316         /* 32-bit registers starting from CLW_DPHYCONTTX */
 317         count = reg < CLW_DPHYCONTTX_REG ? 1 : 2;
 318
 319         err = regmap_bulk_write(tc358746->regmap, reg, &val, count);
 320         if (err)
 321                 dev_err(tc358746->sd.dev,
 322                         "Failed to write reg:0x%04x err:%d\n", reg, err);
 323
 324         return err;
 325 }
 326
 327 static int tc358746_read(struct tc358746 *tc358746, u32 reg, u32 *val)
 328 {
 329         size_t count;
 330         int err;
 331
 332         /* 32-bit registers starting from CLW_DPHYCONTTX */
 333         count = reg < CLW_DPHYCONTTX_REG ? 1 : 2;
 334         *val = 0;
 335
 336         err = regmap_bulk_read(tc358746->regmap, reg, val, count);
 337         if (err)
 338                 dev_err(tc358746->sd.dev,
 339                         "Failed to read reg:0x%04x err:%d\n", reg, err);
 340
 341         return err;
 342 }
 343
 344 static int
 345 tc358746_update_bits(struct tc358746 *tc358746, u32 reg, u32 mask, u32 val)
 346 {
 347         u32 tmp, orig;
 348         int err;
 349
 350         err = tc358746_read(tc358746, reg, &orig);
 351         if (err)
 352                 return err;
 353
 354         tmp = orig & ~mask;
 355         tmp |= val & mask;
 356
 357         return tc358746_write(tc358746, reg, tmp);
 358 }
 359
 360 static int tc358746_set_bits(struct tc358746 *tc358746, u32 reg, u32 bits)
 361 {
 362         return tc358746_update_bits(tc358746, reg, bits, bits);
 363 }
 364
 365 static int tc358746_clear_bits(struct tc358746 *tc358746, u32 reg, u32 bits)
 366 {
 367         return tc358746_update_bits(tc358746, reg, bits, 0);
 368 }
 369
 370 static int tc358746_sw_reset(struct tc358746 *tc358746)
 371 {
 372         int err;
 373
 374         err = tc358746_set_bits(tc358746, SYSCTL_REG, SRESET);
 375         if (err)
 376                 return err;
 377
 378         fsleep(10);
 379
 380         return tc358746_clear_bits(tc358746, SYSCTL_REG, SRESET);
 381 }
 382
 383 static int
 384 tc358746_apply_pll_config(struct tc358746 *tc358746)
 385 {
 386         u8 post = tc358746->pll_post_div;
 387         u16 pre = tc358746->pll_pre_div;
 388         u16 mul = tc358746->pll_mul;
 389         u32 val, mask;
 390         int err;
 391
 392         err = tc358746_read(tc358746, PLLCTL1_REG, &val);
 393         if (err)
 394                 return err;
 395
 396         /* Don't touch the PLL if running */
 397         if (FIELD_GET(PLL_EN, val) == 1)
 398                 return 0;
 399
 400         /* Pre-div and Multiplicator have a internal +1 logic */
 401         val = PLL_PRD(pre - 1) | PLL_FBD(mul - 1);
 402         mask = PLL_PRD_MASK | PLL_FBD_MASK;
 403         err = tc358746_update_bits(tc358746, PLLCTL0_REG, mask, val);
 404         if (err)
 405                 return err;
 406
 407         val = PLL_FRS(ilog2(post)) | RESETB | PLL_EN;
 408         mask = PLL_FRS_MASK | RESETB | PLL_EN;
 409         err = tc358746_update_bits(tc358746, PLLCTL1_REG, mask, val);
 410         if (err)
 411                 return err;
 412
 413         fsleep(1000);
 414
 415         return tc358746_set_bits(tc358746, PLLCTL1_REG, CKEN);
 416 }
 417
 418 static int tc358746_apply_misc_config(struct tc358746 *tc358746)
 419 {
 420         const struct v4l2_mbus_framefmt *mbusfmt;
 421         struct v4l2_subdev *sd = &tc358746->sd;
 422         struct v4l2_subdev_state *sink_state;
 423         const struct tc358746_format *fmt;
 424         struct device *dev = sd->dev;
 425         u32 val;
 426         int err;
 427
 428         sink_state = v4l2_subdev_lock_and_get_active_state(sd);
 429
 430         mbusfmt = v4l2_subdev_state_get_format(sink_state, TC358746_SINK);
 431         fmt = tc358746_get_format_by_code(TC358746_SINK, mbusfmt->code);
 432
 433         /* Self defined CSI user data type id's are not supported yet */
 434         val = PDFMT(fmt->pdformat);
 435         dev_dbg(dev, "DATAFMT: 0x%x\n", val);
 436         err = tc358746_write(tc358746, DATAFMT_REG, val);
 437         if (err)
 438                 goto out;
 439
 440         val = PDATAF(fmt->pdataf);
 441         dev_dbg(dev, "CONFCTL[PDATAF]: 0x%x\n", fmt->pdataf);
 442         err = tc358746_update_bits(tc358746, CONFCTL_REG, PDATAF_MASK, val);
 443         if (err)
 444                 goto out;
 445
 446         val = tc358746->vb_size / 32;
 447         dev_dbg(dev, "FIFOCTL: %u (0x%x)\n", val, val);
 448         err = tc358746_write(tc358746, FIFOCTL_REG, val);
 449         if (err)
 450                 goto out;
 451
 452         /* Total number of bytes for each line/width */
 453         val = mbusfmt->width * fmt->bpp / 8;
 454         dev_dbg(dev, "WORDCNT: %u (0x%x)\n", val, val);
 455         err = tc358746_write(tc358746, WORDCNT_REG, val);
 456
 457 out:
 458         v4l2_subdev_unlock_state(sink_state);
 459
 460         return err;
 461 }
 462
 463 /* Use MHz as base so the div needs no u64 */
 464 static u32 tc358746_cfg_to_cnt(unsigned int cfg_val,
 465                                unsigned int clk_mhz,
 466                                unsigned int time_base)
 467 {
 468         return DIV_ROUND_UP(cfg_val * clk_mhz, time_base);
 469 }
 470
 471 static u32 tc358746_ps_to_cnt(unsigned int cfg_val,
 472                               unsigned int clk_mhz)
 473 {
 474         return tc358746_cfg_to_cnt(cfg_val, clk_mhz, USEC_PER_SEC);
 475 }
 476
 477 static u32 tc358746_us_to_cnt(unsigned int cfg_val,
 478                               unsigned int clk_mhz)
 479 {
 480         return tc358746_cfg_to_cnt(cfg_val, clk_mhz, 1);
 481 }
 482
 483 static int tc358746_apply_dphy_config(struct tc358746 *tc358746)
 484 {
 485         struct phy_configure_opts_mipi_dphy *cfg = &tc358746->dphy_cfg;
 486         bool non_cont_clk = !!(tc358746->csi_vep.bus.mipi_csi2.flags &
 487                                V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK);
 488         struct device *dev = tc358746->sd.dev;
 489         unsigned long hs_byte_clk, hf_clk;
 490         u32 val, val2, lptxcnt;
 491         int err;
 492
 493         /* The hs_byte_clk is also called SYSCLK in the excel sheet */
 494         hs_byte_clk = cfg->hs_clk_rate / 8;
 495         hs_byte_clk /= HZ_PER_MHZ;
 496         hf_clk = hs_byte_clk / 2;
 497
 498         val = tc358746_us_to_cnt(cfg->init, hf_clk) - 1;
 499         dev_dbg(dev, "LINEINITCNT: %u (0x%x)\n", val, val);
 500         err = tc358746_write(tc358746, LINEINITCNT_REG, val);
 501         if (err)
 502                 return err;
 503
 504         val = tc358746_ps_to_cnt(cfg->lpx, hs_byte_clk) - 1;
 505         lptxcnt = val;
 506         dev_dbg(dev, "LPTXTIMECNT: %u (0x%x)\n", val, val);
 507         err = tc358746_write(tc358746, LPTXTIMECNT_REG, val);
 508         if (err)
 509                 return err;
 510
 511         val = tc358746_ps_to_cnt(cfg->clk_prepare, hs_byte_clk) - 1;
 512         val2 = tc358746_ps_to_cnt(cfg->clk_zero, hs_byte_clk) - 1;
 513         dev_dbg(dev, "TCLK_PREPARECNT: %u (0x%x)\n", val, val);
 514         dev_dbg(dev, "TCLK_ZEROCNT: %u (0x%x)\n", val2, val2);
 515         dev_dbg(dev, "TCLK_HEADERCNT: 0x%x\n",
 516                 (u32)(TCLK_PREPARECNT(val) | TCLK_ZEROCNT(val2)));
 517         err = tc358746_write(tc358746, TCLK_HEADERCNT_REG,
 518                              TCLK_PREPARECNT(val) | TCLK_ZEROCNT(val2));
 519         if (err)
 520                 return err;
 521
 522         val = tc358746_ps_to_cnt(cfg->clk_trail, hs_byte_clk);
 523         dev_dbg(dev, "TCLK_TRAILCNT: %u (0x%x)\n", val, val);
 524         err = tc358746_write(tc358746, TCLK_TRAILCNT_REG, val);
 525         if (err)
 526                 return err;
 527
 528         val = tc358746_ps_to_cnt(cfg->hs_prepare, hs_byte_clk) - 1;
 529         val2 = tc358746_ps_to_cnt(cfg->hs_zero, hs_byte_clk) - 1;
 530         dev_dbg(dev, "THS_PREPARECNT: %u (0x%x)\n", val, val);
 531         dev_dbg(dev, "THS_ZEROCNT: %u (0x%x)\n", val2, val2);
 532         dev_dbg(dev, "THS_HEADERCNT: 0x%x\n",
 533                 (u32)(THS_PREPARECNT(val) | THS_ZEROCNT(val2)));
 534         err = tc358746_write(tc358746, THS_HEADERCNT_REG,
 535                              THS_PREPARECNT(val) | THS_ZEROCNT(val2));
 536         if (err)
 537                 return err;
 538
 539         /* TWAKEUP > 1ms in lptxcnt steps */
 540         val = tc358746_us_to_cnt(cfg->wakeup, hs_byte_clk);
 541         val = val / (lptxcnt + 1) - 1;
 542         dev_dbg(dev, "TWAKEUP: %u (0x%x)\n", val, val);
 543         err = tc358746_write(tc358746, TWAKEUP_REG, val);
 544         if (err)
 545                 return err;
 546
 547         val = tc358746_ps_to_cnt(cfg->clk_post, hs_byte_clk);
 548         dev_dbg(dev, "TCLK_POSTCNT: %u (0x%x)\n", val, val);
 549         err = tc358746_write(tc358746, TCLK_POSTCNT_REG, val);
 550         if (err)
 551                 return err;
 552
 553         val = tc358746_ps_to_cnt(cfg->hs_trail, hs_byte_clk);
 554         dev_dbg(dev, "THS_TRAILCNT: %u (0x%x)\n", val, val);
 555         err = tc358746_write(tc358746, THS_TRAILCNT_REG, val);
 556         if (err)
 557                 return err;
 558
 559         dev_dbg(dev, "CONTCLKMODE: %u", non_cont_clk ? 0 : 1);
 560
 561         return  tc358746_write(tc358746, TXOPTIONCNTRL_REG, non_cont_clk ? 0 : 1);
 562 }
 563
 564 #define MAX_DATA_LANES 4
 565
 566 static int tc358746_enable_csi_lanes(struct tc358746 *tc358746, int enable)
 567 {
 568         unsigned int lanes = tc358746->dphy_cfg.lanes;
 569         unsigned int lane;
 570         u32 reg, val;
 571         int err;
 572
 573         err = tc358746_update_bits(tc358746, CONFCTL_REG, DATALANE_MASK,
 574                                    lanes - 1);
 575         if (err)
 576                 return err;
 577
 578         /* Clock lane */
 579         val = enable ? 0 : LANEDISABLE;
 580         dev_dbg(tc358746->sd.dev, "CLW_CNTRL: 0x%x\n", val);
 581         err = tc358746_write(tc358746, CLW_CNTRL_REG, val);
 582         if (err)
 583                 return err;
 584
 585         for (lane = 0; lane < MAX_DATA_LANES; lane++) {
 586                 /* Data lanes */
 587                 reg = D0W_CNTRL_REG + lane * 0x4;
 588                 val = (enable && lane < lanes) ? 0 : LANEDISABLE;
 589
 590                 dev_dbg(tc358746->sd.dev, "D%uW_CNTRL: 0x%x\n", lane, val);
 591                 err = tc358746_write(tc358746, reg, val);
 592                 if (err)
 593                         return err;
 594         }
 595
 596         val = 0;
 597         if (enable) {
 598                 /* Clock lane */
 599                 val |= BIT(0);
 600
 601                 /* Data lanes */
 602                 for (lane = 1; lane <= lanes; lane++)
 603                         val |= BIT(lane);
 604         }
 605
 606         dev_dbg(tc358746->sd.dev, "HSTXVREGEN: 0x%x\n", val);
 607
 608         return tc358746_write(tc358746, HSTXVREGEN_REG, val);
 609 }
 610
 611 static int tc358746_enable_csi_module(struct tc358746 *tc358746, int enable)
 612 {
 613         unsigned int lanes = tc358746->dphy_cfg.lanes;
 614         int err;
 615
 616         /*
 617          * START and STRT are only reseted/disabled by sw reset. This is
 618          * required to put the lane state back into LP-11 state. The sw reset
 619          * don't reset register values.
 620          */
 621         if (!enable)
 622                 return tc358746_sw_reset(tc358746);
 623
 624         err = tc358746_write(tc358746, STARTCNTRL_REG, START);
 625         if (err)
 626                 return err;
 627
 628         err = tc358746_write(tc358746, CSI_START_REG, STRT);
 629         if (err)
 630                 return err;
 631
 632         /* CSI_CONTROL_REG is only indirect accessible */
 633         return tc358746_write(tc358746, CSI_CONFW_REG,
 634                               MODE(MODE_SET) |
 635                               ADDRESS(CSI_CONTROL_ADDRESS) |
 636                               DATA(CSI_MODE | TXHSMD | NOL(lanes - 1)));
 637 }
 638
 639 static int tc358746_enable_parallel_port(struct tc358746 *tc358746, int enable)
 640 {
 641         int err;
 642
 643         if (enable) {
 644                 err = tc358746_write(tc358746, PP_MISC_REG, 0);
 645                 if (err)
 646                         return err;
 647
 648                 return tc358746_set_bits(tc358746, CONFCTL_REG, PPEN);
 649         }
 650
 651         err = tc358746_set_bits(tc358746, PP_MISC_REG, FRMSTOP);
 652         if (err)
 653                 return err;
 654
 655         err = tc358746_clear_bits(tc358746, CONFCTL_REG, PPEN);
 656         if (err)
 657                 return err;
 658
 659         return tc358746_set_bits(tc358746, PP_MISC_REG, RSTPTR);
 660 }
 661
 662 static inline struct v4l2_subdev *tc358746_get_remote_sd(struct media_pad *pad)
 663 {
 664         pad = media_pad_remote_pad_first(pad);
 665         if (!pad)
 666                 return NULL;
 667
 668         return media_entity_to_v4l2_subdev(pad->entity);
 669 }
 670
 671 static int tc358746_s_stream(struct v4l2_subdev *sd, int enable)
 672 {
 673         struct tc358746 *tc358746 = to_tc358746(sd);
 674         struct v4l2_subdev *src;
 675         int err;
 676
 677         dev_dbg(sd->dev, "%sable\n", enable ? "en" : "dis");
 678
 679         src = tc358746_get_remote_sd(&tc358746->pads[TC358746_SINK]);
 680         if (!src)
 681                 return -EPIPE;
 682
 683         if (enable) {
 684                 err = pm_runtime_resume_and_get(sd->dev);
 685                 if (err)
 686                         return err;
 687
 688                 err = tc358746_apply_dphy_config(tc358746);
 689                 if (err)
 690                         goto err_out;
 691
 692                 err = tc358746_apply_misc_config(tc358746);
 693                 if (err)
 694                         goto err_out;
 695
 696                 err = tc358746_enable_csi_lanes(tc358746, 1);
 697                 if (err)
 698                         goto err_out;
 699
 700                 err = tc358746_enable_csi_module(tc358746, 1);
 701                 if (err)
 702                         goto err_out;
 703
 704                 err = tc358746_enable_parallel_port(tc358746, 1);
 705                 if (err)
 706                         goto err_out;
 707
 708                 err = v4l2_subdev_call(src, video, s_stream, 1);
 709                 if (err)
 710                         goto err_out;
 711
 712                 return 0;
 713
 714 err_out:
 715                 pm_runtime_mark_last_busy(sd->dev);
 716                 pm_runtime_put_sync_autosuspend(sd->dev);
 717
 718                 return err;
 719         }
 720
 721         /*
 722          * The lanes must be disabled first (before the csi module) so the
 723          * LP-11 state is entered correctly.
 724          */
 725         err = tc358746_enable_csi_lanes(tc358746, 0);
 726         if (err)
 727                 return err;
 728
 729         err = tc358746_enable_csi_module(tc358746, 0);
 730         if (err)
 731                 return err;
 732
 733         err = tc358746_enable_parallel_port(tc358746, 0);
 734         if (err)
 735                 return err;
 736
 737         pm_runtime_mark_last_busy(sd->dev);
 738         pm_runtime_put_sync_autosuspend(sd->dev);
 739
 740         return v4l2_subdev_call(src, video, s_stream, 0);
 741 }
 742
 743 static int tc358746_init_state(struct v4l2_subdev *sd,
 744                                struct v4l2_subdev_state *state)
 745 {
 746         struct v4l2_mbus_framefmt *fmt;
 747
 748         fmt = v4l2_subdev_state_get_format(state, TC358746_SINK);
 749         *fmt = tc358746_def_fmt;
 750
 751         fmt = v4l2_subdev_state_get_format(state, TC358746_SOURCE);
 752         *fmt = tc358746_def_fmt;
 753         fmt->code = tc358746_src_mbus_code(tc358746_def_fmt.code);
 754
 755         return 0;
 756 }
 757
 758 static int tc358746_enum_mbus_code(struct v4l2_subdev *sd,
 759                                    struct v4l2_subdev_state *sd_state,
 760                                    struct v4l2_subdev_mbus_code_enum *code)
 761 {
 762         const struct tc358746_format *fmt;
 763
 764         fmt = tc358746_get_format_by_idx(code->pad, code->index);
 765         if (IS_ERR(fmt))
 766                 return PTR_ERR(fmt);
 767
 768         code->code = fmt->code;
 769
 770         return 0;
 771 }
 772
 773 static int tc358746_set_fmt(struct v4l2_subdev *sd,
 774                             struct v4l2_subdev_state *sd_state,
 775                             struct v4l2_subdev_format *format)
 776 {
 777         struct v4l2_mbus_framefmt *src_fmt, *sink_fmt;
 778         const struct tc358746_format *fmt;
 779
 780         /* Source follows the sink */
 781         if (format->pad == TC358746_SOURCE)
 782                 return v4l2_subdev_get_fmt(sd, sd_state, format);
 783
 784         sink_fmt = v4l2_subdev_state_get_format(sd_state, TC358746_SINK);
 785
 786         fmt = tc358746_get_format_by_code(format->pad, format->format.code);
 787         if (IS_ERR(fmt)) {
 788                 fmt = tc358746_get_format_by_code(format->pad, tc358746_def_fmt.code);
 789                 // Can't happen, but just in case...
 790                 if (WARN_ON(IS_ERR(fmt)))
 791                         return -EINVAL;
 792         }
 793
 794         format->format.code = fmt->code;
 795         format->format.field = V4L2_FIELD_NONE;
 796
 797         dev_dbg(sd->dev, "Update format: %ux%u code:0x%x -> %ux%u code:0x%x",
 798                 sink_fmt->width, sink_fmt->height, sink_fmt->code,
 799                 format->format.width, format->format.height, format->format.code);
 800
 801         *sink_fmt = format->format;
 802
 803         src_fmt = v4l2_subdev_state_get_format(sd_state, TC358746_SOURCE);
 804         *src_fmt = *sink_fmt;
 805         src_fmt->code = tc358746_src_mbus_code(sink_fmt->code);
 806
 807         return 0;
 808 }
 809
 810 static unsigned long tc358746_find_pll_settings(struct tc358746 *tc358746,
 811                                                 unsigned long refclk,
 812                                                 unsigned long fout)
 813
 814 {
 815         struct device *dev = tc358746->sd.dev;
 816         unsigned long best_freq = 0;
 817         u32 min_delta = 0xffffffff;
 818         u16 prediv_max = 17;
 819         u16 prediv_min = 1;
 820         u16 m_best = 0, mul;
 821         u16 p_best = 1, p;
 822         u8 postdiv;
 823
 824         if (fout > 1000 * HZ_PER_MHZ) {
 825                 dev_err(dev, "HS-Clock above 1 Ghz are not supported\n");
 826                 return 0;
 827         }
 828
 829         if (fout >= 500 * HZ_PER_MHZ)
 830                 postdiv = 1;
 831         else if (fout >= 250 * HZ_PER_MHZ)
 832                 postdiv = 2;
 833         else if (fout >= 125 * HZ_PER_MHZ)
 834                 postdiv = 4;
 835         else
 836                 postdiv = 8;
 837
 838         for (p = prediv_min; p <= prediv_max; p++) {
 839                 unsigned long delta, fin;
 840                 u64 tmp;
 841
 842                 fin = DIV_ROUND_CLOSEST(refclk, p);
 843                 if (fin < 4 * HZ_PER_MHZ || fin > 40 * HZ_PER_MHZ)
 844                         continue;
 845
 846                 tmp = fout * postdiv;
 847                 do_div(tmp, fin);
 848                 mul = tmp;
 849                 if (mul > 511)
 850                         continue;
 851
 852                 tmp = mul * fin;
 853                 do_div(tmp, postdiv);
 854
 855                 delta = abs(fout - tmp);
 856                 if (delta < min_delta) {
 857                         p_best = p;
 858                         m_best = mul;
 859                         min_delta = delta;
 860                         best_freq = tmp;
 861                 }
 862
 863                 if (delta == 0)
 864                         break;
 865         }
 866
 867         if (!best_freq) {
 868                 dev_err(dev, "Failed find PLL frequency\n");
 869                 return 0;
 870         }
 871
 872         tc358746->pll_post_div = postdiv;
 873         tc358746->pll_pre_div = p_best;
 874         tc358746->pll_mul = m_best;
 875
 876         if (best_freq != fout)
 877                 dev_warn(dev, "Request PLL freq:%lu, found PLL freq:%lu\n",
 878                          fout, best_freq);
 879
 880         dev_dbg(dev, "Found PLL settings: freq:%lu prediv:%u multi:%u postdiv:%u\n",
 881                 best_freq, p_best, m_best, postdiv);
 882
 883         return best_freq;
 884 }
 885
 886 #define TC358746_PRECISION 10
 887
 888 static int
 889 tc358746_link_validate(struct v4l2_subdev *sd, struct media_link *link,
 890                        struct v4l2_subdev_format *source_fmt,
 891                        struct v4l2_subdev_format *sink_fmt)
 892 {
 893         struct tc358746 *tc358746 = to_tc358746(sd);
 894         unsigned long csi_bitrate, source_bitrate;
 895         struct v4l2_subdev_state *sink_state;
 896         struct v4l2_mbus_framefmt *mbusfmt;
 897         const struct tc358746_format *fmt;
 898         unsigned int fifo_sz, tmp, n;
 899         struct v4l2_subdev *source;
 900         s64 source_link_freq;
 901         int err;
 902
 903         err = v4l2_subdev_link_validate_default(sd, link, source_fmt, sink_fmt);
 904         if (err)
 905                 return err;
 906
 907         sink_state = v4l2_subdev_lock_and_get_active_state(sd);
 908         mbusfmt = v4l2_subdev_state_get_format(sink_state, TC358746_SINK);
 909
 910         /* Check the FIFO settings */
 911         fmt = tc358746_get_format_by_code(TC358746_SINK, mbusfmt->code);
 912
 913         source = media_entity_to_v4l2_subdev(link->source->entity);
 914         source_link_freq = v4l2_get_link_freq(source->ctrl_handler, 0, 0);
 915         if (source_link_freq <= 0) {
 916                 dev_err(tc358746->sd.dev,
 917                         "Failed to query or invalid source link frequency\n");
 918                 v4l2_subdev_unlock_state(sink_state);
 919                 /* Return -EINVAL in case of source_link_freq is 0 */
 920                 return source_link_freq ? : -EINVAL;
 921         }
 922         source_bitrate = source_link_freq * fmt->bus_width;
 923
 924         csi_bitrate = tc358746->dphy_cfg.lanes * tc358746->pll_rate;
 925
 926         dev_dbg(tc358746->sd.dev,
 927                 "Fifo settings params: source-bitrate:%lu csi-bitrate:%lu",
 928                 source_bitrate, csi_bitrate);
 929
 930         /* Avoid possible FIFO overflows */
 931         if (csi_bitrate < source_bitrate) {
 932                 v4l2_subdev_unlock_state(sink_state);
 933                 return -EINVAL;
 934         }
 935
 936         /* Best case */
 937         if (csi_bitrate == source_bitrate) {
 938                 fifo_sz = TC358746_VB_DEFAULT_SIZE;
 939                 tc358746->vb_size = TC358746_VB_DEFAULT_SIZE;
 940                 goto out;
 941         }
 942
 943         /*
 944          * Avoid possible FIFO underflow in case of
 945          * csi_bitrate > source_bitrate. For such case the chip has a internal
 946          * fifo which can be used to delay the line output.
 947          *
 948          * Fifo size calculation (excluding precision):
 949          *
 950          * fifo-sz, image-width - in bits
 951          * sbr                  - source_bitrate in bits/s
 952          * csir                 - csi_bitrate in bits/s
 953          *
 954          * image-width / csir >= (image-width - fifo-sz) / sbr
 955          * image-width * sbr / csir >= image-width - fifo-sz
 956          * fifo-sz >= image-width - image-width * sbr / csir; with n = csir/sbr
 957          * fifo-sz >= image-width - image-width / n
 958          */
 959
 960         source_bitrate /= TC358746_PRECISION;
 961         n = csi_bitrate / source_bitrate;
 962         tmp = (mbusfmt->width * TC358746_PRECISION) / n;
 963         fifo_sz = mbusfmt->width - tmp;
 964         fifo_sz *= fmt->bpp;
 965         tc358746->vb_size = round_up(fifo_sz, 32);
 966
 967 out:
 968         dev_dbg(tc358746->sd.dev,
 969                 "Found FIFO size[bits]:%u -> aligned to size[bits]:%u\n",
 970                 fifo_sz, tc358746->vb_size);
 971
 972         v4l2_subdev_unlock_state(sink_state);
 973
 974         return tc358746->vb_size > TC358746_VB_MAX_SIZE ? -EINVAL : 0;
 975 }
 976
 977 static int tc358746_get_mbus_config(struct v4l2_subdev *sd, unsigned int pad,
 978                                     struct v4l2_mbus_config *config)
 979 {
 980         struct tc358746 *tc358746 = to_tc358746(sd);
 981
 982         if (pad != TC358746_SOURCE)
 983                 return -EINVAL;
 984
 985         config->type = V4L2_MBUS_CSI2_DPHY;
 986         config->bus.mipi_csi2 = tc358746->csi_vep.bus.mipi_csi2;
 987
 988         return 0;
 989 }
 990
 991 static int __maybe_unused
 992 tc358746_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
 993 {
 994         struct tc358746 *tc358746 = to_tc358746(sd);
 995         u32 val;
 996         int err;
 997
 998         /* 32-bit registers starting from CLW_DPHYCONTTX */
 999         reg->size = reg->reg < CLW_DPHYCONTTX_REG ? 2 : 4;
1000
1001         if (!pm_runtime_get_if_in_use(sd->dev))
1002                 return 0;
1003
1004         err = tc358746_read(tc358746, reg->reg, &val);
1005         reg->val = val;
1006
1007         pm_runtime_mark_last_busy(sd->dev);
1008         pm_runtime_put_sync_autosuspend(sd->dev);
1009
1010         return err;
1011 }
1012
1013 static int __maybe_unused
1014 tc358746_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
1015 {
1016         struct tc358746 *tc358746 = to_tc358746(sd);
1017
1018         if (!pm_runtime_get_if_in_use(sd->dev))
1019                 return 0;
1020
1021         tc358746_write(tc358746, (u32)reg->reg, (u32)reg->val);
1022
1023         pm_runtime_mark_last_busy(sd->dev);
1024         pm_runtime_put_sync_autosuspend(sd->dev);
1025
1026         return 0;
1027 }
1028
1029 static const struct v4l2_subdev_core_ops tc358746_core_ops = {
1030 #ifdef CONFIG_VIDEO_ADV_DEBUG
1031         .g_register = tc358746_g_register,
1032         .s_register = tc358746_s_register,
1033 #endif
1034 };
1035
1036 static const struct v4l2_subdev_video_ops tc358746_video_ops = {
1037         .s_stream = tc358746_s_stream,
1038 };
1039
1040 static const struct v4l2_subdev_pad_ops tc358746_pad_ops = {
1041         .enum_mbus_code = tc358746_enum_mbus_code,
1042         .set_fmt = tc358746_set_fmt,
1043         .get_fmt = v4l2_subdev_get_fmt,
1044         .link_validate = tc358746_link_validate,
1045         .get_mbus_config = tc358746_get_mbus_config,
1046 };
1047
1048 static const struct v4l2_subdev_ops tc358746_ops = {
1049         .core = &tc358746_core_ops,
1050         .video = &tc358746_video_ops,
1051         .pad = &tc358746_pad_ops,
1052 };
1053
1054 static const struct v4l2_subdev_internal_ops tc358746_internal_ops = {
1055         .init_state = tc358746_init_state,
1056 };
1057
1058 static const struct media_entity_operations tc358746_entity_ops = {
1059         .get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1,
1060         .link_validate = v4l2_subdev_link_validate,
1061 };
1062
1063 static int tc358746_mclk_enable(struct clk_hw *hw)
1064 {
1065         struct tc358746 *tc358746 = clk_hw_to_tc358746(hw);
1066         unsigned int div;
1067         u32 val;
1068         int err;
1069
1070         div = tc358746->mclk_postdiv / 2;
1071         val = MCLK_HIGH(div - 1) | MCLK_LOW(div - 1);
1072         dev_dbg(tc358746->sd.dev, "MCLKCTL: %u (0x%x)\n", val, val);
1073         err = tc358746_write(tc358746, MCLKCTL_REG, val);
1074         if (err)
1075                 return err;
1076
1077         if (tc358746->mclk_prediv == 8)
1078                 val = MCLKDIV(MCLKDIV_8);
1079         else if (tc358746->mclk_prediv == 4)
1080                 val = MCLKDIV(MCLKDIV_4);
1081         else
1082                 val = MCLKDIV(MCLKDIV_2);
1083
1084         dev_dbg(tc358746->sd.dev, "CLKCTL[MCLKDIV]: %u (0x%x)\n", val, val);
1085
1086         return tc358746_update_bits(tc358746, CLKCTL_REG, MCLKDIV_MASK, val);
1087 }
1088
1089 static void tc358746_mclk_disable(struct clk_hw *hw)
1090 {
1091         struct tc358746 *tc358746 = clk_hw_to_tc358746(hw);
1092
1093         tc358746_write(tc358746, MCLKCTL_REG, 0);
1094 }
1095
1096 static long
1097 tc358746_find_mclk_settings(struct tc358746 *tc358746, unsigned long mclk_rate)
1098 {
1099         unsigned long pll_rate = tc358746->pll_rate;
1100         const unsigned char prediv[] = { 2, 4, 8 };
1101         unsigned int mclk_prediv, mclk_postdiv;
1102         struct device *dev = tc358746->sd.dev;
1103         unsigned int postdiv, mclkdiv;
1104         unsigned long best_mclk_rate;
1105         unsigned int i;
1106
1107         /*
1108          *                          MCLK-Div
1109          *           -------------------´`---------------------
1110          *          ´                                          `
1111          *         +-------------+     +------------------------+
1112          *         | MCLK-PreDiv |     |       MCLK-PostDiv     |
1113          * PLL --> |   (2/4/8)   | --> | (mclk_low + mclk_high) | --> MCLK
1114          *         +-------------+     +------------------------+
1115          *
1116          * The register value of mclk_low/high is mclk_low/high+1, i.e.:
1117          *   mclk_low/high = 1   --> 2 MCLK-Ref Counts
1118          *   mclk_low/high = 255 --> 256 MCLK-Ref Counts == max.
1119          * If mclk_low and mclk_high are 0 then MCLK is disabled.
1120          *
1121          * Keep it simple and support 50/50 duty cycles only for now,
1122          * so the calc will be:
1123          *
1124          *   MCLK = PLL / (MCLK-PreDiv * 2 * MCLK-PostDiv)
1125          */
1126
1127         if (mclk_rate == tc358746->mclk_rate)
1128                 return mclk_rate;
1129
1130         /* Highest possible rate */
1131         mclkdiv = pll_rate / mclk_rate;
1132         if (mclkdiv <= 8) {
1133                 mclk_prediv = 2;
1134                 mclk_postdiv = 4;
1135                 best_mclk_rate = pll_rate / (2 * 4);
1136                 goto out;
1137         }
1138
1139         /* First check the prediv */
1140         for (i = 0; i < ARRAY_SIZE(prediv); i++) {
1141                 postdiv = mclkdiv / prediv[i];
1142
1143                 if (postdiv % 2)
1144                         continue;
1145
1146                 if (postdiv >= 4 && postdiv <= 512) {
1147                         mclk_prediv = prediv[i];
1148                         mclk_postdiv = postdiv;
1149                         best_mclk_rate = pll_rate / (prediv[i] * postdiv);
1150                         goto out;
1151                 }
1152         }
1153
1154         /* No suitable prediv found, so try to adjust the postdiv */
1155         for (postdiv = 4; postdiv <= 512; postdiv += 2) {
1156                 unsigned int pre;
1157
1158                 pre = mclkdiv / postdiv;
1159                 if (pre == 2 || pre == 4 || pre == 8) {
1160                         mclk_prediv = pre;
1161                         mclk_postdiv = postdiv;
1162                         best_mclk_rate = pll_rate / (pre * postdiv);
1163                         goto out;
1164                 }
1165         }
1166
1167         /* The MCLK <-> PLL gap is to high -> use largest possible div */
1168         mclk_prediv = 8;
1169         mclk_postdiv = 512;
1170         best_mclk_rate = pll_rate / (8 * 512);
1171
1172 out:
1173         tc358746->mclk_prediv = mclk_prediv;
1174         tc358746->mclk_postdiv = mclk_postdiv;
1175         tc358746->mclk_rate = best_mclk_rate;
1176
1177         if (best_mclk_rate != mclk_rate)
1178                 dev_warn(dev, "Request MCLK freq:%lu, found MCLK freq:%lu\n",
1179                          mclk_rate, best_mclk_rate);
1180
1181         dev_dbg(dev, "Found MCLK settings: freq:%lu prediv:%u postdiv:%u\n",
1182                 best_mclk_rate, mclk_prediv, mclk_postdiv);
1183
1184         return best_mclk_rate;
1185 }
1186
1187 static unsigned long
1188 tc358746_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
1189 {
1190         struct tc358746 *tc358746 = clk_hw_to_tc358746(hw);
1191         unsigned int prediv, postdiv;
1192         u32 val;
1193         int err;
1194
1195         err = tc358746_read(tc358746, MCLKCTL_REG, &val);
1196         if (err)
1197                 return 0;
1198
1199         postdiv = FIELD_GET(MCLK_LOW_MASK, val) + 1;
1200         postdiv += FIELD_GET(MCLK_HIGH_MASK, val) + 1;
1201
1202         err = tc358746_read(tc358746, CLKCTL_REG, &val);
1203         if (err)
1204                 return 0;
1205
1206         prediv = FIELD_GET(MCLKDIV_MASK, val);
1207         if (prediv == MCLKDIV_8)
1208                 prediv = 8;
1209         else if (prediv == MCLKDIV_4)
1210                 prediv = 4;
1211         else
1212                 prediv = 2;
1213
1214         return tc358746->pll_rate / (prediv * postdiv);
1215 }
1216
1217 static long tc358746_mclk_round_rate(struct clk_hw *hw, unsigned long rate,
1218                                      unsigned long *parent_rate)
1219 {
1220         struct tc358746 *tc358746 = clk_hw_to_tc358746(hw);
1221
1222         *parent_rate = tc358746->pll_rate;
1223
1224         return tc358746_find_mclk_settings(tc358746, rate);
1225 }
1226
1227 static int tc358746_mclk_set_rate(struct clk_hw *hw, unsigned long rate,
1228                                   unsigned long parent_rate)
1229 {
1230         struct tc358746 *tc358746 = clk_hw_to_tc358746(hw);
1231
1232         tc358746_find_mclk_settings(tc358746, rate);
1233
1234         return tc358746_mclk_enable(hw);
1235 }
1236
1237 static const struct clk_ops tc358746_mclk_ops = {
1238         .enable = tc358746_mclk_enable,
1239         .disable = tc358746_mclk_disable,
1240         .recalc_rate = tc358746_recalc_rate,
1241         .round_rate = tc358746_mclk_round_rate,
1242         .set_rate = tc358746_mclk_set_rate,
1243 };
1244
1245 static int tc358746_setup_mclk_provider(struct tc358746 *tc358746)
1246 {
1247         struct clk_init_data mclk_initdata = { };
1248         struct device *dev = tc358746->sd.dev;
1249         const char *mclk_name;
1250         int err;
1251
1252         /* MCLK clk provider support is optional */
1253         if (!device_property_present(dev, "#clock-cells"))
1254                 return 0;
1255
1256         /* Init to highest possibel MCLK */
1257         tc358746->mclk_postdiv = 512;
1258         tc358746->mclk_prediv = 8;
1259
1260         mclk_name = "tc358746-mclk";
1261         device_property_read_string(dev, "clock-output-names", &mclk_name);
1262
1263         mclk_initdata.name = mclk_name;
1264         mclk_initdata.ops = &tc358746_mclk_ops;
1265         tc358746->mclk_hw.init = &mclk_initdata;
1266
1267         err = devm_clk_hw_register(dev, &tc358746->mclk_hw);
1268         if (err) {
1269                 dev_err(dev, "Failed to register mclk provider\n");
1270                 return err;
1271         }
1272
1273         err = devm_of_clk_add_hw_provider(dev, of_clk_hw_simple_get,
1274                                           &tc358746->mclk_hw);
1275         if (err)
1276                 dev_err(dev, "Failed to add mclk provider\n");
1277
1278         return err;
1279 }
1280
1281 static int
1282 tc358746_init_subdev(struct tc358746 *tc358746, struct i2c_client *client)
1283 {
1284         struct v4l2_subdev *sd = &tc358746->sd;
1285         int err;
1286
1287         v4l2_i2c_subdev_init(sd, client, &tc358746_ops);
1288         sd->internal_ops = &tc358746_internal_ops;
1289         sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1290         sd->entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
1291         sd->entity.ops = &tc358746_entity_ops;
1292
1293         tc358746->pads[TC358746_SINK].flags = MEDIA_PAD_FL_SINK;
1294         tc358746->pads[TC358746_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
1295         err = media_entity_pads_init(&sd->entity, TC358746_NR_PADS,
1296                                      tc358746->pads);
1297         if (err)
1298                 return err;
1299
1300         err = v4l2_subdev_init_finalize(sd);
1301         if (err)
1302                 media_entity_cleanup(&sd->entity);
1303
1304         return err;
1305 }
1306
1307 static int
1308 tc358746_init_output_port(struct tc358746 *tc358746, unsigned long refclk)
1309 {
1310         struct device *dev = tc358746->sd.dev;
1311         struct v4l2_fwnode_endpoint *vep;
1312         unsigned long csi_link_rate;
1313         struct fwnode_handle *ep;
1314         unsigned char csi_lanes;
1315         int err;
1316
1317         ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev), TC358746_SOURCE,
1318                                              0, 0);
1319         if (!ep) {
1320                 dev_err(dev, "Missing endpoint node\n");
1321                 return -EINVAL;
1322         }
1323
1324         /* Currently we only support 'parallel in' -> 'csi out' */
1325         vep = &tc358746->csi_vep;
1326         vep->bus_type = V4L2_MBUS_CSI2_DPHY;
1327         err = v4l2_fwnode_endpoint_alloc_parse(ep, vep);
1328         fwnode_handle_put(ep);
1329         if (err) {
1330                 dev_err(dev, "Failed to parse source endpoint\n");
1331                 return err;
1332         }
1333
1334         csi_lanes = vep->bus.mipi_csi2.num_data_lanes;
1335         if (csi_lanes == 0 || csi_lanes > 4 ||
1336             vep->nr_of_link_frequencies == 0) {
1337                 dev_err(dev, "error: Invalid CSI-2 settings\n");
1338                 err = -EINVAL;
1339                 goto err;
1340         }
1341
1342         /* TODO: Add support to handle multiple link frequencies */
1343         csi_link_rate = (unsigned long)vep->link_frequencies[0];
1344         tc358746->pll_rate = tc358746_find_pll_settings(tc358746, refclk,
1345                                                         csi_link_rate * 2);
1346         if (!tc358746->pll_rate) {
1347                 err = -EINVAL;
1348                 goto err;
1349         }
1350
1351         err = phy_mipi_dphy_get_default_config_for_hsclk(tc358746->pll_rate,
1352                                                 csi_lanes, &tc358746->dphy_cfg);
1353         if (err)
1354                 goto err;
1355
1356         tc358746->vb_size = TC358746_VB_DEFAULT_SIZE;
1357
1358         return 0;
1359
1360 err:
1361         v4l2_fwnode_endpoint_free(vep);
1362
1363         return err;
1364 }
1365
1366 static int tc358746_init_hw(struct tc358746 *tc358746)
1367 {
1368         struct device *dev = tc358746->sd.dev;
1369         unsigned int chipid;
1370         u32 val;
1371         int err;
1372
1373         err = pm_runtime_resume_and_get(dev);
1374         if (err < 0) {
1375                 dev_err(dev, "Failed to resume the device\n");
1376                 return err;
1377         }
1378
1379          /* Ensure that CSI interface is put into LP-11 state */
1380         err = tc358746_sw_reset(tc358746);
1381         if (err) {
1382                 pm_runtime_put_sync(dev);
1383                 dev_err(dev, "Failed to reset the device\n");
1384                 return err;
1385         }
1386
1387         err = tc358746_read(tc358746, CHIPID_REG, &val);
1388         pm_runtime_mark_last_busy(dev);
1389         pm_runtime_put_sync_autosuspend(dev);
1390         if (err)
1391                 return -ENODEV;
1392
1393         chipid = FIELD_GET(CHIPID, val);
1394         if (chipid != 0x44) {
1395                 dev_err(dev, "Invalid chipid 0x%02x\n", chipid);
1396                 return -ENODEV;
1397         }
1398
1399         return 0;
1400 }
1401
1402 static int tc358746_init_controls(struct tc358746 *tc358746)
1403 {
1404         u64 *link_frequencies = tc358746->csi_vep.link_frequencies;
1405         struct v4l2_ctrl *ctrl;
1406         int err;
1407
1408         err = v4l2_ctrl_handler_init(&tc358746->ctrl_hdl, 1);
1409         if (err)
1410                 return err;
1411
1412         /*
1413          * The driver currently supports only one link-frequency, regardless of
1414          * the input from the firmware, see: tc358746_init_output_port(). So
1415          * report only the first frequency from the array of possible given
1416          * frequencies.
1417          */
1418         ctrl = v4l2_ctrl_new_int_menu(&tc358746->ctrl_hdl, NULL,
1419                                       V4L2_CID_LINK_FREQ, 0, 0,
1420                                       link_frequencies);
1421         if (ctrl)
1422                 ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1423
1424         err = tc358746->ctrl_hdl.error;
1425         if (err) {
1426                 v4l2_ctrl_handler_free(&tc358746->ctrl_hdl);
1427                 return err;
1428         }
1429
1430         tc358746->sd.ctrl_handler = &tc358746->ctrl_hdl;
1431
1432         return 0;
1433 }
1434
1435 static int tc358746_notify_bound(struct v4l2_async_notifier *notifier,
1436                                  struct v4l2_subdev *sd,
1437                                  struct v4l2_async_connection *asd)
1438 {
1439         struct tc358746 *tc358746 =
1440                 container_of(notifier, struct tc358746, notifier);
1441         u32 flags = MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE;
1442         struct media_pad *sink = &tc358746->pads[TC358746_SINK];
1443
1444         return v4l2_create_fwnode_links_to_pad(sd, sink, flags);
1445 }
1446
1447 static const struct v4l2_async_notifier_operations tc358746_notify_ops = {
1448         .bound = tc358746_notify_bound,
1449 };
1450
1451 static int tc358746_async_register(struct tc358746 *tc358746)
1452 {
1453         struct v4l2_fwnode_endpoint vep = {
1454                 .bus_type = V4L2_MBUS_PARALLEL,
1455         };
1456         struct v4l2_async_connection *asd;
1457         struct fwnode_handle *ep;
1458         int err;
1459
1460         ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(tc358746->sd.dev),
1461                                              TC358746_SINK, 0, 0);
1462         if (!ep)
1463                 return -ENOTCONN;
1464
1465         err = v4l2_fwnode_endpoint_parse(ep, &vep);
1466         if (err) {
1467                 fwnode_handle_put(ep);
1468                 return err;
1469         }
1470
1471         v4l2_async_subdev_nf_init(&tc358746->notifier, &tc358746->sd);
1472         asd = v4l2_async_nf_add_fwnode_remote(&tc358746->notifier, ep,
1473                                               struct v4l2_async_connection);
1474         fwnode_handle_put(ep);
1475
1476         if (IS_ERR(asd)) {
1477                 err = PTR_ERR(asd);
1478                 goto err_cleanup;
1479         }
1480
1481         tc358746->notifier.ops = &tc358746_notify_ops;
1482
1483         err = v4l2_async_nf_register(&tc358746->notifier);
1484         if (err)
1485                 goto err_cleanup;
1486
1487         err = v4l2_async_register_subdev(&tc358746->sd);
1488         if (err)
1489                 goto err_unregister;
1490
1491         return 0;
1492
1493 err_unregister:
1494         v4l2_async_nf_unregister(&tc358746->notifier);
1495 err_cleanup:
1496         v4l2_async_nf_cleanup(&tc358746->notifier);
1497
1498         return err;
1499 }
1500
1501 static int tc358746_probe(struct i2c_client *client)
1502 {
1503         struct device *dev = &client->dev;
1504         struct tc358746 *tc358746;
1505         unsigned long refclk;
1506         unsigned int i;
1507         int err;
1508
1509         tc358746 = devm_kzalloc(&client->dev, sizeof(*tc358746), GFP_KERNEL);
1510         if (!tc358746)
1511                 return -ENOMEM;
1512
1513         tc358746->regmap = devm_regmap_init_i2c(client, &tc358746_regmap_config);
1514         if (IS_ERR(tc358746->regmap))
1515                 return dev_err_probe(dev, PTR_ERR(tc358746->regmap),
1516                                      "Failed to init regmap\n");
1517
1518         tc358746->refclk = devm_clk_get(dev, "refclk");
1519         if (IS_ERR(tc358746->refclk))
1520                 return dev_err_probe(dev, PTR_ERR(tc358746->refclk),
1521                                      "Failed to get refclk\n");
1522
1523         err = clk_prepare_enable(tc358746->refclk);
1524         if (err)
1525                 return dev_err_probe(dev, err,
1526                                      "Failed to enable refclk\n");
1527
1528         refclk = clk_get_rate(tc358746->refclk);
1529         clk_disable_unprepare(tc358746->refclk);
1530
1531         if (refclk < 6 * HZ_PER_MHZ || refclk > 40 * HZ_PER_MHZ)
1532                 return dev_err_probe(dev, -EINVAL, "Invalid refclk range\n");
1533
1534         for (i = 0; i < ARRAY_SIZE(tc358746_supplies); i++)
1535                 tc358746->supplies[i].supply = tc358746_supplies[i];
1536
1537         err = devm_regulator_bulk_get(dev, ARRAY_SIZE(tc358746_supplies),
1538                                       tc358746->supplies);
1539         if (err)
1540                 return dev_err_probe(dev, err, "Failed to get supplies\n");
1541
1542         tc358746->reset_gpio = devm_gpiod_get_optional(dev, "reset",
1543                                                        GPIOD_OUT_HIGH);
1544         if (IS_ERR(tc358746->reset_gpio))
1545                 return dev_err_probe(dev, PTR_ERR(tc358746->reset_gpio),
1546                                      "Failed to get reset-gpios\n");
1547
1548         err = tc358746_init_subdev(tc358746, client);
1549         if (err)
1550                 return dev_err_probe(dev, err, "Failed to init subdev\n");
1551
1552         err = tc358746_init_output_port(tc358746, refclk);
1553         if (err)
1554                 goto err_subdev;
1555
1556         /*
1557          * Keep this order since we need the output port link-frequencies
1558          * information.
1559          */
1560         err = tc358746_init_controls(tc358746);
1561         if (err)
1562                 goto err_fwnode;
1563
1564         dev_set_drvdata(dev, tc358746);
1565
1566         /* Set to 1sec to give the stream reconfiguration enough time */
1567         pm_runtime_set_autosuspend_delay(dev, 1000);
1568         pm_runtime_use_autosuspend(dev);
1569         pm_runtime_enable(dev);
1570
1571         err = tc358746_init_hw(tc358746);
1572         if (err)
1573                 goto err_pm;
1574
1575         err = tc358746_setup_mclk_provider(tc358746);
1576         if (err)
1577                 goto err_pm;
1578
1579         err = tc358746_async_register(tc358746);
1580         if (err < 0)
1581                 goto err_pm;
1582
1583         dev_dbg(dev, "%s found @ 0x%x (%s)\n", client->name,
1584                 client->addr, client->adapter->name);
1585
1586         return 0;
1587
1588 err_pm:
1589         pm_runtime_disable(dev);
1590         pm_runtime_set_suspended(dev);
1591         pm_runtime_dont_use_autosuspend(dev);
1592         v4l2_ctrl_handler_free(&tc358746->ctrl_hdl);
1593 err_fwnode:
1594         v4l2_fwnode_endpoint_free(&tc358746->csi_vep);
1595 err_subdev:
1596         v4l2_subdev_cleanup(&tc358746->sd);
1597         media_entity_cleanup(&tc358746->sd.entity);
1598
1599         return err;
1600 }
1601
1602 static void tc358746_remove(struct i2c_client *client)
1603 {
1604         struct v4l2_subdev *sd = i2c_get_clientdata(client);
1605         struct tc358746 *tc358746 = to_tc358746(sd);
1606
1607         v4l2_subdev_cleanup(sd);
1608         v4l2_ctrl_handler_free(&tc358746->ctrl_hdl);
1609         v4l2_fwnode_endpoint_free(&tc358746->csi_vep);
1610         v4l2_async_nf_unregister(&tc358746->notifier);
1611         v4l2_async_nf_cleanup(&tc358746->notifier);
1612         v4l2_async_unregister_subdev(sd);
1613         media_entity_cleanup(&sd->entity);
1614
1615         pm_runtime_disable(sd->dev);
1616         pm_runtime_set_suspended(sd->dev);
1617         pm_runtime_dont_use_autosuspend(sd->dev);
1618 }
1619
1620 static int tc358746_suspend(struct device *dev)
1621 {
1622         struct tc358746 *tc358746 = dev_get_drvdata(dev);
1623         int err;
1624
1625         clk_disable_unprepare(tc358746->refclk);
1626
1627         err = regulator_bulk_disable(ARRAY_SIZE(tc358746_supplies),
1628                                      tc358746->supplies);
1629         if (err)
1630                 clk_prepare_enable(tc358746->refclk);
1631
1632         return err;
1633 }
1634
1635 static int tc358746_resume(struct device *dev)
1636 {
1637         struct tc358746 *tc358746 = dev_get_drvdata(dev);
1638         int err;
1639
1640         gpiod_set_value(tc358746->reset_gpio, 1);
1641
1642         err = regulator_bulk_enable(ARRAY_SIZE(tc358746_supplies),
1643                                     tc358746->supplies);
1644         if (err)
1645                 return err;
1646
1647         /* min. 200ns */
1648         usleep_range(10, 20);
1649
1650         gpiod_set_value(tc358746->reset_gpio, 0);
1651
1652         err = clk_prepare_enable(tc358746->refclk);
1653         if (err)
1654                 goto err;
1655
1656         /* min. 700us ... 1ms */
1657         usleep_range(1000, 1500);
1658
1659         /*
1660          * Enable the PLL here since it can be called by the clk-framework or by
1661          * the .s_stream() callback. So this is the common place for both.
1662          */
1663         err = tc358746_apply_pll_config(tc358746);
1664         if (err)
1665                 goto err_clk;
1666
1667         return 0;
1668
1669 err_clk:
1670         clk_disable_unprepare(tc358746->refclk);
1671 err:
1672         regulator_bulk_disable(ARRAY_SIZE(tc358746_supplies),
1673                                tc358746->supplies);
1674         return err;
1675 }
1676
1677 static DEFINE_RUNTIME_DEV_PM_OPS(tc358746_pm_ops, tc358746_suspend,
1678                                  tc358746_resume, NULL);
1679
1680 static const struct of_device_id __maybe_unused tc358746_of_match[] = {
1681         { .compatible = "toshiba,tc358746" },
1682         { },
1683 };
1684 MODULE_DEVICE_TABLE(of, tc358746_of_match);
1685
1686 static struct i2c_driver tc358746_driver = {
1687         .driver = {
1688                 .name = "tc358746",
1689                 .pm = pm_ptr(&tc358746_pm_ops),
1690                 .of_match_table = tc358746_of_match,
1691         },
1692         .probe = tc358746_probe,
1693         .remove = tc358746_remove,
1694 };
1695
1696 module_i2c_driver(tc358746_driver);
1697
1698 MODULE_DESCRIPTION("Toshiba TC358746 Parallel to CSI-2 bridge driver");
1699 MODULE_AUTHOR("Marco Felsch <kernel@pengutronix.de>");
1700 MODULE_LICENSE("GPL");