drivers/media/i2c/max96717.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Maxim GMSL2 Serializer Driver
   4  *
   5  * Copyright (C) 2024 Collabora Ltd.
   6  */
   7
   8 #include <linux/bitfield.h>
   9 #include <linux/clk.h>
  10 #include <linux/clk-provider.h>
  11 #include <linux/delay.h>
  12 #include <linux/fwnode.h>
  13 #include <linux/gpio/driver.h>
  14 #include <linux/i2c-mux.h>
  15 #include <linux/i2c.h>
  16 #include <linux/regmap.h>
  17
  18 #include <media/v4l2-cci.h>
  19 #include <media/v4l2-fwnode.h>
  20 #include <media/v4l2-subdev.h>
  21
  22 #define MAX96717_DEVICE_ID  0xbf
  23 #define MAX96717F_DEVICE_ID 0xc8
  24 #define MAX96717_PORTS      2
  25 #define MAX96717_PAD_SINK   0
  26 #define MAX96717_PAD_SOURCE 1
  27
  28 #define MAX96717_DEFAULT_CLKOUT_RATE    24000000UL
  29
  30 /* DEV */
  31 #define MAX96717_REG3    CCI_REG8(0x3)
  32 #define MAX96717_RCLKSEL GENMASK(1, 0)
  33 #define RCLKSEL_REF_PLL  CCI_REG8(0x3)
  34 #define MAX96717_REG6    CCI_REG8(0x6)
  35 #define RCLKEN           BIT(5)
  36 #define MAX96717_DEV_ID  CCI_REG8(0xd)
  37 #define MAX96717_DEV_REV CCI_REG8(0xe)
  38 #define MAX96717_DEV_REV_MASK GENMASK(3, 0)
  39
  40 /* VID_TX Z */
  41 #define MAX96717_VIDEO_TX2 CCI_REG8(0x112)
  42 #define MAX96717_VIDEO_PCLKDET BIT(7)
  43
  44 /* GPIO */
  45 #define MAX96717_NUM_GPIO         11
  46 #define MAX96717_GPIO_REG_A(gpio) CCI_REG8(0x2be + (gpio) * 3)
  47 #define MAX96717_GPIO_OUT         BIT(4)
  48 #define MAX96717_GPIO_IN          BIT(3)
  49 #define MAX96717_GPIO_RX_EN       BIT(2)
  50 #define MAX96717_GPIO_TX_EN       BIT(1)
  51 #define MAX96717_GPIO_OUT_DIS     BIT(0)
  52
  53 /* FRONTTOP */
  54 /* MAX96717 only have CSI port 'B' */
  55 #define MAX96717_FRONTOP0     CCI_REG8(0x308)
  56 #define MAX96717_START_PORT_B BIT(5)
  57
  58 /* MIPI_RX */
  59 #define MAX96717_MIPI_RX1       CCI_REG8(0x331)
  60 #define MAX96717_MIPI_LANES_CNT GENMASK(5, 4)
  61 #define MAX96717_MIPI_RX2       CCI_REG8(0x332) /* phy1 Lanes map */
  62 #define MAX96717_PHY2_LANES_MAP GENMASK(7, 4)
  63 #define MAX96717_MIPI_RX3       CCI_REG8(0x333) /* phy2 Lanes map */
  64 #define MAX96717_PHY1_LANES_MAP GENMASK(3, 0)
  65 #define MAX96717_MIPI_RX4       CCI_REG8(0x334) /* phy1 lane polarities */
  66 #define MAX96717_PHY1_LANES_POL GENMASK(6, 4)
  67 #define MAX96717_MIPI_RX5       CCI_REG8(0x335) /* phy2 lane polarities */
  68 #define MAX96717_PHY2_LANES_POL GENMASK(2, 0)
  69
  70 /* MIPI_RX_EXT */
  71 #define MAX96717_MIPI_RX_EXT11 CCI_REG8(0x383)
  72 #define MAX96717_TUN_MODE      BIT(7)
  73
  74 /* REF_VTG */
  75 #define REF_VTG0                CCI_REG8(0x3f0)
  76 #define REFGEN_PREDEF_EN        BIT(6)
  77 #define REFGEN_PREDEF_FREQ_MASK GENMASK(5, 4)
  78 #define REFGEN_PREDEF_FREQ_ALT  BIT(3)
  79 #define REFGEN_RST              BIT(1)
  80 #define REFGEN_EN               BIT(0)
  81
  82 /* MISC */
  83 #define PIO_SLEW_1 CCI_REG8(0x570)
  84
  85 struct max96717_priv {
  86         struct i2c_client                 *client;
  87         struct regmap                     *regmap;
  88         struct i2c_mux_core               *mux;
  89         struct v4l2_mbus_config_mipi_csi2 mipi_csi2;
  90         struct v4l2_subdev                sd;
  91         struct media_pad                  pads[MAX96717_PORTS];
  92         struct v4l2_async_notifier        notifier;
  93         struct v4l2_subdev                *source_sd;
  94         u16                               source_sd_pad;
  95         u64                               enabled_source_streams;
  96         u8                                pll_predef_index;
  97         struct clk_hw                     clk_hw;
  98         struct gpio_chip                  gpio_chip;
  99 };
 100
 101 static inline struct max96717_priv *sd_to_max96717(struct v4l2_subdev *sd)
 102 {
 103         return container_of(sd, struct max96717_priv, sd);
 104 }
 105
 106 static inline struct max96717_priv *clk_hw_to_max96717(struct clk_hw *hw)
 107 {
 108         return container_of(hw, struct max96717_priv, clk_hw);
 109 }
 110
 111 static int max96717_i2c_mux_select(struct i2c_mux_core *mux, u32 chan)
 112 {
 113         return 0;
 114 }
 115
 116 static int max96717_i2c_mux_init(struct max96717_priv *priv)
 117 {
 118         priv->mux = i2c_mux_alloc(priv->client->adapter, &priv->client->dev,
 119                                   1, 0, I2C_MUX_LOCKED | I2C_MUX_GATE,
 120                                   max96717_i2c_mux_select, NULL);
 121         if (!priv->mux)
 122                 return -ENOMEM;
 123
 124         return i2c_mux_add_adapter(priv->mux, 0, 0);
 125 }
 126
 127 static inline int max96717_start_csi(struct max96717_priv *priv, bool start)
 128 {
 129         return cci_update_bits(priv->regmap, MAX96717_FRONTOP0,
 130                                MAX96717_START_PORT_B,
 131                                start ? MAX96717_START_PORT_B : 0, NULL);
 132 }
 133
 134 static int max96717_gpiochip_get(struct gpio_chip *gpiochip,
 135                                  unsigned int offset)
 136 {
 137         struct max96717_priv *priv = gpiochip_get_data(gpiochip);
 138         u64 val;
 139         int ret;
 140
 141         ret = cci_read(priv->regmap, MAX96717_GPIO_REG_A(offset),
 142                        &val, NULL);
 143         if (ret)
 144                 return ret;
 145
 146         if (val & MAX96717_GPIO_OUT_DIS)
 147                 return !!(val & MAX96717_GPIO_IN);
 148         else
 149                 return !!(val & MAX96717_GPIO_OUT);
 150 }
 151
 152 static void max96717_gpiochip_set(struct gpio_chip *gpiochip,
 153                                   unsigned int offset, int value)
 154 {
 155         struct max96717_priv *priv = gpiochip_get_data(gpiochip);
 156
 157         cci_update_bits(priv->regmap, MAX96717_GPIO_REG_A(offset),
 158                         MAX96717_GPIO_OUT, MAX96717_GPIO_OUT, NULL);
 159 }
 160
 161 static int max96717_gpio_get_direction(struct gpio_chip *gpiochip,
 162                                        unsigned int offset)
 163 {
 164         struct max96717_priv *priv = gpiochip_get_data(gpiochip);
 165         u64 val;
 166         int ret;
 167
 168         ret = cci_read(priv->regmap, MAX96717_GPIO_REG_A(offset), &val, NULL);
 169         if (ret < 0)
 170                 return ret;
 171
 172         return !!(val & MAX96717_GPIO_OUT_DIS);
 173 }
 174
 175 static int max96717_gpio_direction_out(struct gpio_chip *gpiochip,
 176                                        unsigned int offset, int value)
 177 {
 178         struct max96717_priv *priv = gpiochip_get_data(gpiochip);
 179
 180         return cci_update_bits(priv->regmap, MAX96717_GPIO_REG_A(offset),
 181                                MAX96717_GPIO_OUT_DIS | MAX96717_GPIO_OUT,
 182                                value ? MAX96717_GPIO_OUT : 0, NULL);
 183 }
 184
 185 static int max96717_gpio_direction_in(struct gpio_chip *gpiochip,
 186                                       unsigned int offset)
 187 {
 188         struct max96717_priv *priv = gpiochip_get_data(gpiochip);
 189
 190         return cci_update_bits(priv->regmap, MAX96717_GPIO_REG_A(offset),
 191                                MAX96717_GPIO_OUT_DIS, MAX96717_GPIO_OUT_DIS,
 192                                NULL);
 193 }
 194
 195 static int max96717_gpiochip_probe(struct max96717_priv *priv)
 196 {
 197         struct device *dev = &priv->client->dev;
 198         struct gpio_chip *gc = &priv->gpio_chip;
 199         int i, ret = 0;
 200
 201         gc->label = dev_name(dev);
 202         gc->parent = dev;
 203         gc->owner = THIS_MODULE;
 204         gc->ngpio = MAX96717_NUM_GPIO;
 205         gc->base = -1;
 206         gc->can_sleep = true;
 207         gc->get_direction = max96717_gpio_get_direction;
 208         gc->direction_input = max96717_gpio_direction_in;
 209         gc->direction_output = max96717_gpio_direction_out;
 210         gc->set = max96717_gpiochip_set;
 211         gc->get = max96717_gpiochip_get;
 212         gc->of_gpio_n_cells = 2;
 213
 214         /* Disable GPIO forwarding */
 215         for (i = 0; i < gc->ngpio; i++)
 216                 cci_update_bits(priv->regmap, MAX96717_GPIO_REG_A(i),
 217                                 MAX96717_GPIO_RX_EN | MAX96717_GPIO_TX_EN,
 218                                 0, &ret);
 219
 220         if (ret)
 221                 return ret;
 222
 223         ret = devm_gpiochip_add_data(dev, gc, priv);
 224         if (ret) {
 225                 dev_err(dev, "Unable to create gpio_chip\n");
 226                 return ret;
 227         }
 228
 229         return 0;
 230 }
 231
 232 static int _max96717_set_routing(struct v4l2_subdev *sd,
 233                                  struct v4l2_subdev_state *state,
 234                                  struct v4l2_subdev_krouting *routing)
 235 {
 236         static const struct v4l2_mbus_framefmt format = {
 237                 .width = 1280,
 238                 .height = 1080,
 239                 .code = MEDIA_BUS_FMT_Y8_1X8,
 240                 .field = V4L2_FIELD_NONE,
 241         };
 242         int ret;
 243
 244         ret = v4l2_subdev_routing_validate(sd, routing,
 245                                            V4L2_SUBDEV_ROUTING_ONLY_1_TO_1);
 246         if (ret)
 247                 return ret;
 248
 249         ret = v4l2_subdev_set_routing_with_fmt(sd, state, routing, &format);
 250         if (ret)
 251                 return ret;
 252
 253         return 0;
 254 }
 255
 256 static int max96717_set_routing(struct v4l2_subdev *sd,
 257                                 struct v4l2_subdev_state *state,
 258                                 enum v4l2_subdev_format_whence which,
 259                                 struct v4l2_subdev_krouting *routing)
 260 {
 261         struct max96717_priv *priv = sd_to_max96717(sd);
 262
 263         if (which == V4L2_SUBDEV_FORMAT_ACTIVE && priv->enabled_source_streams)
 264                 return -EBUSY;
 265
 266         return _max96717_set_routing(sd, state, routing);
 267 }
 268
 269 static int max96717_set_fmt(struct v4l2_subdev *sd,
 270                             struct v4l2_subdev_state *state,
 271                             struct v4l2_subdev_format *format)
 272 {
 273         struct max96717_priv *priv = sd_to_max96717(sd);
 274         struct v4l2_mbus_framefmt *fmt;
 275         u64 stream_source_mask;
 276
 277         if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE &&
 278             priv->enabled_source_streams)
 279                 return -EBUSY;
 280
 281         /* No transcoding, source and sink formats must match. */
 282         if (format->pad == MAX96717_PAD_SOURCE)
 283                 return v4l2_subdev_get_fmt(sd, state, format);
 284
 285         /* Set sink format */
 286         fmt = v4l2_subdev_state_get_format(state, format->pad, format->stream);
 287         if (!fmt)
 288                 return -EINVAL;
 289
 290         *fmt = format->format;
 291
 292         /* Propagate to source format */
 293         fmt = v4l2_subdev_state_get_opposite_stream_format(state, format->pad,
 294                                                            format->stream);
 295         if (!fmt)
 296                 return -EINVAL;
 297         *fmt = format->format;
 298
 299         stream_source_mask = BIT(format->stream);
 300
 301         return v4l2_subdev_state_xlate_streams(state, MAX96717_PAD_SOURCE,
 302                                                MAX96717_PAD_SINK,
 303                                                &stream_source_mask);
 304 }
 305
 306 static int max96717_init_state(struct v4l2_subdev *sd,
 307                                struct v4l2_subdev_state *state)
 308 {
 309         struct v4l2_subdev_route routes[] = {
 310                 {
 311                         .sink_pad = MAX96717_PAD_SINK,
 312                         .sink_stream = 0,
 313                         .source_pad = MAX96717_PAD_SOURCE,
 314                         .source_stream = 0,
 315                         .flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE,
 316                 },
 317         };
 318         struct v4l2_subdev_krouting routing = {
 319                 .num_routes = ARRAY_SIZE(routes),
 320                 .routes = routes,
 321         };
 322
 323         return _max96717_set_routing(sd, state, &routing);
 324 }
 325
 326 static bool max96717_pipe_pclkdet(struct max96717_priv *priv)
 327 {
 328         u64 val = 0;
 329
 330         cci_read(priv->regmap, MAX96717_VIDEO_TX2, &val, NULL);
 331
 332         return val & MAX96717_VIDEO_PCLKDET;
 333 }
 334
 335 static int max96717_log_status(struct v4l2_subdev *sd)
 336 {
 337         struct max96717_priv *priv = sd_to_max96717(sd);
 338         struct device *dev = &priv->client->dev;
 339
 340         dev_info(dev, "Serializer: max96717\n");
 341         dev_info(dev, "Pipe: pclkdet:%d\n", max96717_pipe_pclkdet(priv));
 342
 343         return 0;
 344 }
 345
 346 static int max96717_enable_streams(struct v4l2_subdev *sd,
 347                                    struct v4l2_subdev_state *state, u32 pad,
 348                                    u64 streams_mask)
 349 {
 350         struct max96717_priv *priv = sd_to_max96717(sd);
 351         struct device *dev = &priv->client->dev;
 352         u64 sink_streams;
 353         int ret;
 354
 355         sink_streams = v4l2_subdev_state_xlate_streams(state,
 356                                                        MAX96717_PAD_SOURCE,
 357                                                        MAX96717_PAD_SINK,
 358                                                        &streams_mask);
 359
 360         if (!priv->enabled_source_streams)
 361                 max96717_start_csi(priv, true);
 362
 363         ret = v4l2_subdev_enable_streams(priv->source_sd, priv->source_sd_pad,
 364                                          sink_streams);
 365         if (ret) {
 366                 dev_err(dev, "Fail to start streams:%llu on remote subdev\n",
 367                         sink_streams);
 368                 goto stop_csi;
 369         }
 370
 371         priv->enabled_source_streams |= streams_mask;
 372
 373         return 0;
 374
 375 stop_csi:
 376         if (!priv->enabled_source_streams)
 377                 max96717_start_csi(priv, false);
 378         return ret;
 379 }
 380
 381 static int max96717_disable_streams(struct v4l2_subdev *sd,
 382                                     struct v4l2_subdev_state *state, u32 pad,
 383                                     u64 streams_mask)
 384 {
 385         struct max96717_priv *priv = sd_to_max96717(sd);
 386         u64 sink_streams;
 387
 388         /*
 389          * Stop the CSI receiver first then the source,
 390          * otherwise the device may become unresponsive
 391          * while holding the I2C bus low.
 392          */
 393         priv->enabled_source_streams &= ~streams_mask;
 394         if (!priv->enabled_source_streams)
 395                 max96717_start_csi(priv, false);
 396
 397         sink_streams = v4l2_subdev_state_xlate_streams(state,
 398                                                        MAX96717_PAD_SOURCE,
 399                                                        MAX96717_PAD_SINK,
 400                                                        &streams_mask);
 401
 402         return v4l2_subdev_disable_streams(priv->source_sd, priv->source_sd_pad,
 403                                            sink_streams);
 404 }
 405
 406 static const struct v4l2_subdev_pad_ops max96717_pad_ops = {
 407         .enable_streams = max96717_enable_streams,
 408         .disable_streams = max96717_disable_streams,
 409         .set_routing = max96717_set_routing,
 410         .get_fmt = v4l2_subdev_get_fmt,
 411         .set_fmt = max96717_set_fmt,
 412 };
 413
 414 static const struct v4l2_subdev_core_ops max96717_subdev_core_ops = {
 415         .log_status = max96717_log_status,
 416 };
 417
 418 static const struct v4l2_subdev_internal_ops max96717_internal_ops = {
 419         .init_state = max96717_init_state,
 420 };
 421
 422 static const struct v4l2_subdev_ops max96717_subdev_ops = {
 423         .core = &max96717_subdev_core_ops,
 424         .pad = &max96717_pad_ops,
 425 };
 426
 427 static const struct media_entity_operations max96717_entity_ops = {
 428         .link_validate = v4l2_subdev_link_validate,
 429 };
 430
 431 static int max96717_notify_bound(struct v4l2_async_notifier *notifier,
 432                                  struct v4l2_subdev *source_subdev,
 433                                  struct v4l2_async_connection *asd)
 434 {
 435         struct max96717_priv *priv = sd_to_max96717(notifier->sd);
 436         struct device *dev = &priv->client->dev;
 437         int ret;
 438
 439         ret = media_entity_get_fwnode_pad(&source_subdev->entity,
 440                                           source_subdev->fwnode,
 441                                           MEDIA_PAD_FL_SOURCE);
 442         if (ret < 0) {
 443                 dev_err(dev, "Failed to find pad for %s\n",
 444                         source_subdev->name);
 445                 return ret;
 446         }
 447
 448         priv->source_sd = source_subdev;
 449         priv->source_sd_pad = ret;
 450
 451         ret = media_create_pad_link(&source_subdev->entity, priv->source_sd_pad,
 452                                     &priv->sd.entity, 0,
 453                                     MEDIA_LNK_FL_ENABLED |
 454                                     MEDIA_LNK_FL_IMMUTABLE);
 455         if (ret) {
 456                 dev_err(dev, "Unable to link %s:%u -> %s:0\n",
 457                         source_subdev->name, priv->source_sd_pad,
 458                         priv->sd.name);
 459                 return ret;
 460         }
 461
 462         return 0;
 463 }
 464
 465 static const struct v4l2_async_notifier_operations max96717_notify_ops = {
 466         .bound = max96717_notify_bound,
 467 };
 468
 469 static int max96717_v4l2_notifier_register(struct max96717_priv *priv)
 470 {
 471         struct device *dev = &priv->client->dev;
 472         struct v4l2_async_connection *asd;
 473         struct fwnode_handle *ep_fwnode;
 474         int ret;
 475
 476         ep_fwnode = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev),
 477                                                     MAX96717_PAD_SINK, 0, 0);
 478         if (!ep_fwnode) {
 479                 dev_err(dev, "No graph endpoint\n");
 480                 return -ENODEV;
 481         }
 482
 483         v4l2_async_subdev_nf_init(&priv->notifier, &priv->sd);
 484
 485         asd = v4l2_async_nf_add_fwnode_remote(&priv->notifier, ep_fwnode,
 486                                               struct v4l2_async_connection);
 487
 488         fwnode_handle_put(ep_fwnode);
 489
 490         if (IS_ERR(asd)) {
 491                 dev_err(dev, "Failed to add subdev: %ld", PTR_ERR(asd));
 492                 v4l2_async_nf_cleanup(&priv->notifier);
 493                 return PTR_ERR(asd);
 494         }
 495
 496         priv->notifier.ops = &max96717_notify_ops;
 497
 498         ret = v4l2_async_nf_register(&priv->notifier);
 499         if (ret) {
 500                 dev_err(dev, "Failed to register subdev_notifier");
 501                 v4l2_async_nf_cleanup(&priv->notifier);
 502                 return ret;
 503         }
 504
 505         return 0;
 506 }
 507
 508 static int max96717_subdev_init(struct max96717_priv *priv)
 509 {
 510         struct device *dev = &priv->client->dev;
 511         int ret;
 512
 513         v4l2_i2c_subdev_init(&priv->sd, priv->client, &max96717_subdev_ops);
 514         priv->sd.internal_ops = &max96717_internal_ops;
 515
 516         priv->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_STREAMS;
 517         priv->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
 518         priv->sd.entity.ops = &max96717_entity_ops;
 519
 520         priv->pads[MAX96717_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
 521         priv->pads[MAX96717_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
 522
 523         ret = media_entity_pads_init(&priv->sd.entity, 2, priv->pads);
 524         if (ret)
 525                 return dev_err_probe(dev, ret, "Failed to init pads\n");
 526
 527         ret = v4l2_subdev_init_finalize(&priv->sd);
 528         if (ret) {
 529                 dev_err_probe(dev, ret,
 530                               "v4l2 subdev init finalized failed\n");
 531                 goto err_entity_cleanup;
 532         }
 533         ret = max96717_v4l2_notifier_register(priv);
 534         if (ret) {
 535                 dev_err_probe(dev, ret,
 536                               "v4l2 subdev notifier register failed\n");
 537                 goto err_free_state;
 538         }
 539
 540         ret = v4l2_async_register_subdev(&priv->sd);
 541         if (ret) {
 542                 dev_err_probe(dev, ret, "v4l2_async_register_subdev error\n");
 543                 goto err_unreg_notif;
 544         }
 545
 546         return 0;
 547
 548 err_unreg_notif:
 549         v4l2_async_nf_unregister(&priv->notifier);
 550         v4l2_async_nf_cleanup(&priv->notifier);
 551 err_free_state:
 552         v4l2_subdev_cleanup(&priv->sd);
 553 err_entity_cleanup:
 554         media_entity_cleanup(&priv->sd.entity);
 555
 556         return ret;
 557 }
 558
 559 static void max96717_subdev_uninit(struct max96717_priv *priv)
 560 {
 561         v4l2_async_unregister_subdev(&priv->sd);
 562         v4l2_async_nf_unregister(&priv->notifier);
 563         v4l2_async_nf_cleanup(&priv->notifier);
 564         v4l2_subdev_cleanup(&priv->sd);
 565         media_entity_cleanup(&priv->sd.entity);
 566 }
 567
 568 struct max96717_pll_predef_freq {
 569         unsigned long freq;
 570         bool is_alt;
 571         u8 val;
 572 };
 573
 574 static const struct max96717_pll_predef_freq max96717_predef_freqs[] = {
 575         { 13500000, true,  0 }, { 19200000, false, 0 },
 576         { 24000000, true,  1 }, { 27000000, false, 1 },
 577         { 37125000, false, 2 }, { 74250000, false, 3 },
 578 };
 579
 580 static unsigned long
 581 max96717_clk_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
 582 {
 583         struct max96717_priv *priv = clk_hw_to_max96717(hw);
 584
 585         return max96717_predef_freqs[priv->pll_predef_index].freq;
 586 }
 587
 588 static unsigned int max96717_clk_find_best_index(struct max96717_priv *priv,
 589                                                  unsigned long rate)
 590 {
 591         unsigned int i, idx;
 592         unsigned long diff_new, diff_old;
 593
 594         diff_old = U32_MAX;
 595         idx = 0;
 596
 597         for (i = 0; i < ARRAY_SIZE(max96717_predef_freqs); i++) {
 598                 diff_new = abs(rate - max96717_predef_freqs[i].freq);
 599                 if (diff_new < diff_old) {
 600                         diff_old = diff_new;
 601                         idx = i;
 602                 }
 603         }
 604
 605         return idx;
 606 }
 607
 608 static long max96717_clk_round_rate(struct clk_hw *hw, unsigned long rate,
 609                                     unsigned long *parent_rate)
 610 {
 611         struct max96717_priv *priv = clk_hw_to_max96717(hw);
 612         struct device *dev = &priv->client->dev;
 613         unsigned int idx;
 614
 615         idx = max96717_clk_find_best_index(priv, rate);
 616
 617         if (rate != max96717_predef_freqs[idx].freq) {
 618                 dev_warn(dev, "Request CLK freq:%lu, found CLK freq:%lu\n",
 619                          rate, max96717_predef_freqs[idx].freq);
 620         }
 621
 622         return max96717_predef_freqs[idx].freq;
 623 }
 624
 625 static int max96717_clk_set_rate(struct clk_hw *hw, unsigned long rate,
 626                                  unsigned long parent_rate)
 627 {
 628         struct max96717_priv *priv = clk_hw_to_max96717(hw);
 629         unsigned int val, idx;
 630         int ret = 0;
 631
 632         idx = max96717_clk_find_best_index(priv, rate);
 633
 634         val = FIELD_PREP(REFGEN_PREDEF_FREQ_MASK,
 635                          max96717_predef_freqs[idx].val);
 636
 637         if (max96717_predef_freqs[idx].is_alt)
 638                 val |= REFGEN_PREDEF_FREQ_ALT;
 639
 640         val |= REFGEN_RST | REFGEN_PREDEF_EN;
 641
 642         cci_write(priv->regmap, REF_VTG0, val, &ret);
 643         cci_update_bits(priv->regmap, REF_VTG0, REFGEN_RST | REFGEN_EN,
 644                         REFGEN_EN, &ret);
 645         if (ret)
 646                 return ret;
 647
 648         priv->pll_predef_index = idx;
 649
 650         return 0;
 651 }
 652
 653 static int max96717_clk_prepare(struct clk_hw *hw)
 654 {
 655         struct max96717_priv *priv = clk_hw_to_max96717(hw);
 656
 657         return cci_update_bits(priv->regmap, MAX96717_REG6, RCLKEN,
 658                                RCLKEN, NULL);
 659 }
 660
 661 static void max96717_clk_unprepare(struct clk_hw *hw)
 662 {
 663         struct max96717_priv *priv = clk_hw_to_max96717(hw);
 664
 665         cci_update_bits(priv->regmap, MAX96717_REG6, RCLKEN, 0, NULL);
 666 }
 667
 668 static const struct clk_ops max96717_clk_ops = {
 669         .prepare     = max96717_clk_prepare,
 670         .unprepare   = max96717_clk_unprepare,
 671         .set_rate    = max96717_clk_set_rate,
 672         .recalc_rate = max96717_clk_recalc_rate,
 673         .round_rate  = max96717_clk_round_rate,
 674 };
 675
 676 static int max96717_register_clkout(struct max96717_priv *priv)
 677 {
 678         struct device *dev = &priv->client->dev;
 679         struct clk_init_data init = { .ops = &max96717_clk_ops };
 680         int ret;
 681
 682         init.name = kasprintf(GFP_KERNEL, "max96717.%s.clk_out",
 683                               dev_name(dev));
 684         if (!init.name)
 685                 return -ENOMEM;
 686
 687         /* RCLKSEL Reference PLL output */
 688         ret = cci_update_bits(priv->regmap, MAX96717_REG3, MAX96717_RCLKSEL,
 689                               MAX96717_RCLKSEL, NULL);
 690         /* MFP4 fastest slew rate */
 691         cci_update_bits(priv->regmap, PIO_SLEW_1, BIT(5) | BIT(4), 0, &ret);
 692         if (ret)
 693                 goto free_init_name;
 694
 695         priv->clk_hw.init = &init;
 696
 697         /* Initialize to 24 MHz */
 698         ret = max96717_clk_set_rate(&priv->clk_hw,
 699                                     MAX96717_DEFAULT_CLKOUT_RATE, 0);
 700         if (ret < 0)
 701                 goto free_init_name;
 702
 703         ret = devm_clk_hw_register(dev, &priv->clk_hw);
 704         kfree(init.name);
 705         if (ret)
 706                 return dev_err_probe(dev, ret, "Cannot register clock HW\n");
 707
 708         ret = devm_of_clk_add_hw_provider(dev, of_clk_hw_simple_get,
 709                                           &priv->clk_hw);
 710         if (ret)
 711                 return dev_err_probe(dev, ret,
 712                                      "Cannot add OF clock provider\n");
 713
 714         return 0;
 715
 716 free_init_name:
 717         kfree(init.name);
 718         return ret;
 719 }
 720
 721 static int max96717_init_csi_lanes(struct max96717_priv *priv)
 722 {
 723         struct v4l2_mbus_config_mipi_csi2 *mipi = &priv->mipi_csi2;
 724         unsigned long lanes_used = 0;
 725         unsigned int nlanes, lane, val = 0;
 726         int ret;
 727
 728         nlanes = mipi->num_data_lanes;
 729
 730         ret = cci_update_bits(priv->regmap, MAX96717_MIPI_RX1,
 731                               MAX96717_MIPI_LANES_CNT,
 732                               FIELD_PREP(MAX96717_MIPI_LANES_CNT,
 733                                          nlanes - 1), NULL);
 734
 735         /* lanes polarity */
 736         for (lane = 0; lane < nlanes + 1; lane++) {
 737                 if (!mipi->lane_polarities[lane])
 738                         continue;
 739                 /* Clock lane */
 740                 if (lane == 0)
 741                         val |= BIT(2);
 742                 else if (lane < 3)
 743                         val |= BIT(lane - 1);
 744                 else
 745                         val |= BIT(lane);
 746         }
 747
 748         cci_update_bits(priv->regmap, MAX96717_MIPI_RX5,
 749                         MAX96717_PHY2_LANES_POL,
 750                         FIELD_PREP(MAX96717_PHY2_LANES_POL, val), &ret);
 751
 752         cci_update_bits(priv->regmap, MAX96717_MIPI_RX4,
 753                         MAX96717_PHY1_LANES_POL,
 754                         FIELD_PREP(MAX96717_PHY1_LANES_POL,
 755                                    val >> 3), &ret);
 756         /* lanes mapping */
 757         for (lane = 0, val = 0; lane < nlanes; lane++) {
 758                 val |= (mipi->data_lanes[lane] - 1) << (lane * 2);
 759                 lanes_used |= BIT(mipi->data_lanes[lane] - 1);
 760         }
 761
 762         /*
 763          * Unused lanes need to be mapped as well to not have
 764          * the same lanes mapped twice.
 765          */
 766         for (; lane < 4; lane++) {
 767                 unsigned int idx = find_first_zero_bit(&lanes_used, 4);
 768
 769                 val |= idx << (lane * 2);
 770                 lanes_used |= BIT(idx);
 771         }
 772
 773         cci_update_bits(priv->regmap, MAX96717_MIPI_RX3,
 774                         MAX96717_PHY1_LANES_MAP,
 775                         FIELD_PREP(MAX96717_PHY1_LANES_MAP, val), &ret);
 776
 777         return cci_update_bits(priv->regmap, MAX96717_MIPI_RX2,
 778                                MAX96717_PHY2_LANES_MAP,
 779                                FIELD_PREP(MAX96717_PHY2_LANES_MAP, val >> 4),
 780                                &ret);
 781 }
 782
 783 static int max96717_hw_init(struct max96717_priv *priv)
 784 {
 785         struct device *dev = &priv->client->dev;
 786         u64 dev_id, val;
 787         int ret;
 788
 789         ret = cci_read(priv->regmap, MAX96717_DEV_ID, &dev_id, NULL);
 790         if (ret)
 791                 return dev_err_probe(dev, ret,
 792                                      "Fail to read the device id\n");
 793
 794         if (dev_id != MAX96717_DEVICE_ID && dev_id != MAX96717F_DEVICE_ID)
 795                 return dev_err_probe(dev, -EOPNOTSUPP,
 796                                      "Unsupported device id got %x\n", (u8)dev_id);
 797
 798         ret = cci_read(priv->regmap, MAX96717_DEV_REV, &val, NULL);
 799         if (ret)
 800                 return dev_err_probe(dev, ret,
 801                                      "Fail to read device revision");
 802
 803         dev_dbg(dev, "Found %x (rev %lx)\n", (u8)dev_id,
 804                 (u8)val & MAX96717_DEV_REV_MASK);
 805
 806         ret = cci_read(priv->regmap, MAX96717_MIPI_RX_EXT11, &val, NULL);
 807         if (ret)
 808                 return dev_err_probe(dev, ret,
 809                                      "Fail to read mipi rx extension");
 810
 811         if (!(val & MAX96717_TUN_MODE))
 812                 return dev_err_probe(dev, -EOPNOTSUPP,
 813                                      "Only supporting tunnel mode");
 814
 815         return max96717_init_csi_lanes(priv);
 816 }
 817
 818 static int max96717_parse_dt(struct max96717_priv *priv)
 819 {
 820         struct device *dev = &priv->client->dev;
 821         struct v4l2_fwnode_endpoint vep = {
 822                 .bus_type = V4L2_MBUS_CSI2_DPHY
 823         };
 824         struct fwnode_handle *ep_fwnode;
 825         unsigned char num_data_lanes;
 826         int ret;
 827
 828         ep_fwnode = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev),
 829                                                     MAX96717_PAD_SINK, 0, 0);
 830         if (!ep_fwnode)
 831                 return dev_err_probe(dev, -ENOENT, "no endpoint found\n");
 832
 833         ret = v4l2_fwnode_endpoint_parse(ep_fwnode, &vep);
 834
 835         fwnode_handle_put(ep_fwnode);
 836
 837         if (ret < 0)
 838                 return dev_err_probe(dev, ret, "Failed to parse sink endpoint");
 839
 840         num_data_lanes = vep.bus.mipi_csi2.num_data_lanes;
 841         if (num_data_lanes < 1 || num_data_lanes > 4)
 842                 return dev_err_probe(dev, -EINVAL,
 843                                      "Invalid data lanes must be 1 to 4\n");
 844
 845         memcpy(&priv->mipi_csi2, &vep.bus.mipi_csi2, sizeof(priv->mipi_csi2));
 846
 847         return 0;
 848 }
 849
 850 static int max96717_probe(struct i2c_client *client)
 851 {
 852         struct device *dev = &client->dev;
 853         struct max96717_priv *priv;
 854         int ret;
 855
 856         priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
 857         if (!priv)
 858                 return -ENOMEM;
 859
 860         priv->client = client;
 861         priv->regmap = devm_cci_regmap_init_i2c(client, 16);
 862         if (IS_ERR(priv->regmap)) {
 863                 ret = PTR_ERR(priv->regmap);
 864                 return dev_err_probe(dev, ret, "Failed to init regmap\n");
 865         }
 866
 867         ret = max96717_parse_dt(priv);
 868         if (ret)
 869                 return dev_err_probe(dev, ret, "Failed to parse the dt\n");
 870
 871         ret = max96717_hw_init(priv);
 872         if (ret)
 873                 return dev_err_probe(dev, ret,
 874                                      "Failed to initialize the hardware\n");
 875
 876         ret = max96717_gpiochip_probe(priv);
 877         if (ret)
 878                 return dev_err_probe(&client->dev, ret,
 879                                      "Failed to init gpiochip\n");
 880
 881         ret = max96717_register_clkout(priv);
 882         if (ret)
 883                 return dev_err_probe(dev, ret, "Failed to register clkout\n");
 884
 885         ret = max96717_subdev_init(priv);
 886         if (ret)
 887                 return dev_err_probe(dev, ret,
 888                                      "Failed to initialize v4l2 subdev\n");
 889
 890         ret = max96717_i2c_mux_init(priv);
 891         if (ret) {
 892                 dev_err_probe(dev, ret, "failed to add remote i2c adapter\n");
 893                 max96717_subdev_uninit(priv);
 894         }
 895
 896         return ret;
 897 }
 898
 899 static void max96717_remove(struct i2c_client *client)
 900 {
 901         struct v4l2_subdev *sd = i2c_get_clientdata(client);
 902         struct max96717_priv *priv = sd_to_max96717(sd);
 903
 904         max96717_subdev_uninit(priv);
 905         i2c_mux_del_adapters(priv->mux);
 906 }
 907
 908 static const struct of_device_id max96717_of_ids[] = {
 909         { .compatible = "maxim,max96717f" },
 910         { }
 911 };
 912 MODULE_DEVICE_TABLE(of, max96717_of_ids);
 913
 914 static struct i2c_driver max96717_i2c_driver = {
 915         .driver = {
 916                 .name           = "max96717",
 917                 .of_match_table = max96717_of_ids,
 918         },
 919         .probe          = max96717_probe,
 920         .remove         = max96717_remove,
 921 };
 922
 923 module_i2c_driver(max96717_i2c_driver);
 924
 925 MODULE_DESCRIPTION("Maxim GMSL2 MAX96717 Serializer Driver");
 926 MODULE_AUTHOR("Julien Massot <[email protected]>");
 927 MODULE_LICENSE("GPL");