drivers/net/pcs/pcs-xpcs.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Copyright (c) 2020 Synopsys, Inc. and/or its affiliates.
   4  * Synopsys DesignWare XPCS helpers
   5  *
   6  * Author: Jose Abreu <Jose.Abreu@synopsys.com>
   7  */
   8
   9 #include <linux/delay.h>
  10 #include <linux/pcs/pcs-xpcs.h>
  11 #include <linux/mdio.h>
  12 #include <linux/phylink.h>
  13 #include <linux/workqueue.h>
  14
  15 #define SYNOPSYS_XPCS_USXGMII_ID        0x7996ced0
  16 #define SYNOPSYS_XPCS_10GKR_ID          0x7996ced0
  17 #define SYNOPSYS_XPCS_XLGMII_ID         0x7996ced0
  18 #define SYNOPSYS_XPCS_MASK              0xffffffff
  19
  20 /* Vendor regs access */
  21 #define DW_VENDOR                       BIT(15)
  22
  23 /* VR_XS_PCS */
  24 #define DW_USXGMII_RST                  BIT(10)
  25 #define DW_USXGMII_EN                   BIT(9)
  26 #define DW_VR_XS_PCS_DIG_STS            0x0010
  27 #define DW_RXFIFO_ERR                   GENMASK(6, 5)
  28
  29 /* SR_MII */
  30 #define DW_USXGMII_FULL                 BIT(8)
  31 #define DW_USXGMII_SS_MASK              (BIT(13) | BIT(6) | BIT(5))
  32 #define DW_USXGMII_10000                (BIT(13) | BIT(6))
  33 #define DW_USXGMII_5000                 (BIT(13) | BIT(5))
  34 #define DW_USXGMII_2500                 (BIT(5))
  35 #define DW_USXGMII_1000                 (BIT(6))
  36 #define DW_USXGMII_100                  (BIT(13))
  37 #define DW_USXGMII_10                   (0)
  38
  39 /* SR_AN */
  40 #define DW_SR_AN_ADV1                   0x10
  41 #define DW_SR_AN_ADV2                   0x11
  42 #define DW_SR_AN_ADV3                   0x12
  43 #define DW_SR_AN_LP_ABL1                0x13
  44 #define DW_SR_AN_LP_ABL2                0x14
  45 #define DW_SR_AN_LP_ABL3                0x15
  46
  47 /* Clause 73 Defines */
  48 /* AN_LP_ABL1 */
  49 #define DW_C73_PAUSE                    BIT(10)
  50 #define DW_C73_ASYM_PAUSE               BIT(11)
  51 #define DW_C73_AN_ADV_SF                0x1
  52 /* AN_LP_ABL2 */
  53 #define DW_C73_1000KX                   BIT(5)
  54 #define DW_C73_10000KX4                 BIT(6)
  55 #define DW_C73_10000KR                  BIT(7)
  56 /* AN_LP_ABL3 */
  57 #define DW_C73_2500KX                   BIT(0)
  58 #define DW_C73_5000KR                   BIT(1)
  59
  60 static const int xpcs_usxgmii_features[] = {
  61         ETHTOOL_LINK_MODE_Pause_BIT,
  62         ETHTOOL_LINK_MODE_Asym_Pause_BIT,
  63         ETHTOOL_LINK_MODE_Autoneg_BIT,
  64         ETHTOOL_LINK_MODE_1000baseKX_Full_BIT,
  65         ETHTOOL_LINK_MODE_10000baseKX4_Full_BIT,
  66         ETHTOOL_LINK_MODE_10000baseKR_Full_BIT,
  67         ETHTOOL_LINK_MODE_2500baseX_Full_BIT,
  68         __ETHTOOL_LINK_MODE_MASK_NBITS,
  69 };
  70
  71 static const int xpcs_10gkr_features[] = {
  72         ETHTOOL_LINK_MODE_Pause_BIT,
  73         ETHTOOL_LINK_MODE_Asym_Pause_BIT,
  74         ETHTOOL_LINK_MODE_10000baseKR_Full_BIT,
  75         __ETHTOOL_LINK_MODE_MASK_NBITS,
  76 };
  77
  78 static const int xpcs_xlgmii_features[] = {
  79         ETHTOOL_LINK_MODE_Pause_BIT,
  80         ETHTOOL_LINK_MODE_Asym_Pause_BIT,
  81         ETHTOOL_LINK_MODE_25000baseCR_Full_BIT,
  82         ETHTOOL_LINK_MODE_25000baseKR_Full_BIT,
  83         ETHTOOL_LINK_MODE_25000baseSR_Full_BIT,
  84         ETHTOOL_LINK_MODE_40000baseKR4_Full_BIT,
  85         ETHTOOL_LINK_MODE_40000baseCR4_Full_BIT,
  86         ETHTOOL_LINK_MODE_40000baseSR4_Full_BIT,
  87         ETHTOOL_LINK_MODE_40000baseLR4_Full_BIT,
  88         ETHTOOL_LINK_MODE_50000baseCR2_Full_BIT,
  89         ETHTOOL_LINK_MODE_50000baseKR2_Full_BIT,
  90         ETHTOOL_LINK_MODE_50000baseSR2_Full_BIT,
  91         ETHTOOL_LINK_MODE_50000baseKR_Full_BIT,
  92         ETHTOOL_LINK_MODE_50000baseSR_Full_BIT,
  93         ETHTOOL_LINK_MODE_50000baseCR_Full_BIT,
  94         ETHTOOL_LINK_MODE_50000baseLR_ER_FR_Full_BIT,
  95         ETHTOOL_LINK_MODE_50000baseDR_Full_BIT,
  96         ETHTOOL_LINK_MODE_100000baseKR4_Full_BIT,
  97         ETHTOOL_LINK_MODE_100000baseSR4_Full_BIT,
  98         ETHTOOL_LINK_MODE_100000baseCR4_Full_BIT,
  99         ETHTOOL_LINK_MODE_100000baseLR4_ER4_Full_BIT,
 100         ETHTOOL_LINK_MODE_100000baseKR2_Full_BIT,
 101         ETHTOOL_LINK_MODE_100000baseSR2_Full_BIT,
 102         ETHTOOL_LINK_MODE_100000baseCR2_Full_BIT,
 103         ETHTOOL_LINK_MODE_100000baseLR2_ER2_FR2_Full_BIT,
 104         ETHTOOL_LINK_MODE_100000baseDR2_Full_BIT,
 105         __ETHTOOL_LINK_MODE_MASK_NBITS,
 106 };
 107
 108 static const phy_interface_t xpcs_usxgmii_interfaces[] = {
 109         PHY_INTERFACE_MODE_USXGMII,
 110         PHY_INTERFACE_MODE_MAX,
 111 };
 112
 113 static const phy_interface_t xpcs_10gkr_interfaces[] = {
 114         PHY_INTERFACE_MODE_10GKR,
 115         PHY_INTERFACE_MODE_MAX,
 116 };
 117
 118 static const phy_interface_t xpcs_xlgmii_interfaces[] = {
 119         PHY_INTERFACE_MODE_XLGMII,
 120         PHY_INTERFACE_MODE_MAX,
 121 };
 122
 123 static struct xpcs_id {
 124         u32 id;
 125         u32 mask;
 126         const int *supported;
 127         const phy_interface_t *interface;
 128 } xpcs_id_list[] = {
 129         {
 130                 .id = SYNOPSYS_XPCS_USXGMII_ID,
 131                 .mask = SYNOPSYS_XPCS_MASK,
 132                 .supported = xpcs_usxgmii_features,
 133                 .interface = xpcs_usxgmii_interfaces,
 134         }, {
 135                 .id = SYNOPSYS_XPCS_10GKR_ID,
 136                 .mask = SYNOPSYS_XPCS_MASK,
 137                 .supported = xpcs_10gkr_features,
 138                 .interface = xpcs_10gkr_interfaces,
 139         }, {
 140                 .id = SYNOPSYS_XPCS_XLGMII_ID,
 141                 .mask = SYNOPSYS_XPCS_MASK,
 142                 .supported = xpcs_xlgmii_features,
 143                 .interface = xpcs_xlgmii_interfaces,
 144         },
 145 };
 146
 147 static int xpcs_read(struct mdio_xpcs_args *xpcs, int dev, u32 reg)
 148 {
 149         u32 reg_addr = MII_ADDR_C45 | dev << 16 | reg;
 150
 151         return mdiobus_read(xpcs->bus, xpcs->addr, reg_addr);
 152 }
 153
 154 static int xpcs_write(struct mdio_xpcs_args *xpcs, int dev, u32 reg, u16 val)
 155 {
 156         u32 reg_addr = MII_ADDR_C45 | dev << 16 | reg;
 157
 158         return mdiobus_write(xpcs->bus, xpcs->addr, reg_addr, val);
 159 }
 160
 161 static int xpcs_read_vendor(struct mdio_xpcs_args *xpcs, int dev, u32 reg)
 162 {
 163         return xpcs_read(xpcs, dev, DW_VENDOR | reg);
 164 }
 165
 166 static int xpcs_write_vendor(struct mdio_xpcs_args *xpcs, int dev, int reg,
 167                              u16 val)
 168 {
 169         return xpcs_write(xpcs, dev, DW_VENDOR | reg, val);
 170 }
 171
 172 static int xpcs_read_vpcs(struct mdio_xpcs_args *xpcs, int reg)
 173 {
 174         return xpcs_read_vendor(xpcs, MDIO_MMD_PCS, reg);
 175 }
 176
 177 static int xpcs_write_vpcs(struct mdio_xpcs_args *xpcs, int reg, u16 val)
 178 {
 179         return xpcs_write_vendor(xpcs, MDIO_MMD_PCS, reg, val);
 180 }
 181
 182 static int xpcs_poll_reset(struct mdio_xpcs_args *xpcs, int dev)
 183 {
 184         /* Poll until the reset bit clears (50ms per retry == 0.6 sec) */
 185         unsigned int retries = 12;
 186         int ret;
 187
 188         do {
 189                 msleep(50);
 190                 ret = xpcs_read(xpcs, dev, MDIO_CTRL1);
 191                 if (ret < 0)
 192                         return ret;
 193         } while (ret & MDIO_CTRL1_RESET && --retries);
 194
 195         return (ret & MDIO_CTRL1_RESET) ? -ETIMEDOUT : 0;
 196 }
 197
 198 static int xpcs_soft_reset(struct mdio_xpcs_args *xpcs, int dev)
 199 {
 200         int ret;
 201
 202         ret = xpcs_write(xpcs, dev, MDIO_CTRL1, MDIO_CTRL1_RESET);
 203         if (ret < 0)
 204                 return ret;
 205
 206         return xpcs_poll_reset(xpcs, dev);
 207 }
 208
 209 #define xpcs_warn(__xpcs, __state, __args...) \
 210 ({ \
 211         if ((__state)->link) \
 212                 dev_warn(&(__xpcs)->bus->dev, ##__args); \
 213 })
 214
 215 static int xpcs_read_fault(struct mdio_xpcs_args *xpcs,
 216                            struct phylink_link_state *state)
 217 {
 218         int ret;
 219
 220         ret = xpcs_read(xpcs, MDIO_MMD_PCS, MDIO_STAT1);
 221         if (ret < 0)
 222                 return ret;
 223
 224         if (ret & MDIO_STAT1_FAULT) {
 225                 xpcs_warn(xpcs, state, "Link fault condition detected!\n");
 226                 return -EFAULT;
 227         }
 228
 229         ret = xpcs_read(xpcs, MDIO_MMD_PCS, MDIO_STAT2);
 230         if (ret < 0)
 231                 return ret;
 232
 233         if (ret & MDIO_STAT2_RXFAULT)
 234                 xpcs_warn(xpcs, state, "Receiver fault detected!\n");
 235         if (ret & MDIO_STAT2_TXFAULT)
 236                 xpcs_warn(xpcs, state, "Transmitter fault detected!\n");
 237
 238         ret = xpcs_read_vendor(xpcs, MDIO_MMD_PCS, DW_VR_XS_PCS_DIG_STS);
 239         if (ret < 0)
 240                 return ret;
 241
 242         if (ret & DW_RXFIFO_ERR) {
 243                 xpcs_warn(xpcs, state, "FIFO fault condition detected!\n");
 244                 return -EFAULT;
 245         }
 246
 247         ret = xpcs_read(xpcs, MDIO_MMD_PCS, MDIO_PCS_10GBRT_STAT1);
 248         if (ret < 0)
 249                 return ret;
 250
 251         if (!(ret & MDIO_PCS_10GBRT_STAT1_BLKLK))
 252                 xpcs_warn(xpcs, state, "Link is not locked!\n");
 253
 254         ret = xpcs_read(xpcs, MDIO_MMD_PCS, MDIO_PCS_10GBRT_STAT2);
 255         if (ret < 0)
 256                 return ret;
 257
 258         if (ret & MDIO_PCS_10GBRT_STAT2_ERR) {
 259                 xpcs_warn(xpcs, state, "Link has errors!\n");
 260                 return -EFAULT;
 261         }
 262
 263         return 0;
 264 }
 265
 266 static int xpcs_read_link(struct mdio_xpcs_args *xpcs, bool an)
 267 {
 268         bool link = true;
 269         int ret;
 270
 271         ret = xpcs_read(xpcs, MDIO_MMD_PCS, MDIO_STAT1);
 272         if (ret < 0)
 273                 return ret;
 274
 275         if (!(ret & MDIO_STAT1_LSTATUS))
 276                 link = false;
 277
 278         if (an) {
 279                 ret = xpcs_read(xpcs, MDIO_MMD_AN, MDIO_STAT1);
 280                 if (ret < 0)
 281                         return ret;
 282
 283                 if (!(ret & MDIO_STAT1_LSTATUS))
 284                         link = false;
 285         }
 286
 287         return link;
 288 }
 289
 290 static int xpcs_get_max_usxgmii_speed(const unsigned long *supported)
 291 {
 292         int max = SPEED_UNKNOWN;
 293
 294         if (phylink_test(supported, 1000baseKX_Full))
 295                 max = SPEED_1000;
 296         if (phylink_test(supported, 2500baseX_Full))
 297                 max = SPEED_2500;
 298         if (phylink_test(supported, 10000baseKX4_Full))
 299                 max = SPEED_10000;
 300         if (phylink_test(supported, 10000baseKR_Full))
 301                 max = SPEED_10000;
 302
 303         return max;
 304 }
 305
 306 static int xpcs_config_usxgmii(struct mdio_xpcs_args *xpcs, int speed)
 307 {
 308         int ret, speed_sel;
 309
 310         switch (speed) {
 311         case SPEED_10:
 312                 speed_sel = DW_USXGMII_10;
 313                 break;
 314         case SPEED_100:
 315                 speed_sel = DW_USXGMII_100;
 316                 break;
 317         case SPEED_1000:
 318                 speed_sel = DW_USXGMII_1000;
 319                 break;
 320         case SPEED_2500:
 321                 speed_sel = DW_USXGMII_2500;
 322                 break;
 323         case SPEED_5000:
 324                 speed_sel = DW_USXGMII_5000;
 325                 break;
 326         case SPEED_10000:
 327                 speed_sel = DW_USXGMII_10000;
 328                 break;
 329         default:
 330                 /* Nothing to do here */
 331                 return -EINVAL;
 332         }
 333
 334         ret = xpcs_read_vpcs(xpcs, MDIO_CTRL1);
 335         if (ret < 0)
 336                 return ret;
 337
 338         ret = xpcs_write_vpcs(xpcs, MDIO_CTRL1, ret | DW_USXGMII_EN);
 339         if (ret < 0)
 340                 return ret;
 341
 342         ret = xpcs_read(xpcs, MDIO_MMD_VEND2, MDIO_CTRL1);
 343         if (ret < 0)
 344                 return ret;
 345
 346         ret &= ~DW_USXGMII_SS_MASK;
 347         ret |= speed_sel | DW_USXGMII_FULL;
 348
 349         ret = xpcs_write(xpcs, MDIO_MMD_VEND2, MDIO_CTRL1, ret);
 350         if (ret < 0)
 351                 return ret;
 352
 353         ret = xpcs_read_vpcs(xpcs, MDIO_CTRL1);
 354         if (ret < 0)
 355                 return ret;
 356
 357         return xpcs_write_vpcs(xpcs, MDIO_CTRL1, ret | DW_USXGMII_RST);
 358 }
 359
 360 static int xpcs_config_aneg_c73(struct mdio_xpcs_args *xpcs)
 361 {
 362         int ret, adv;
 363
 364         /* By default, in USXGMII mode XPCS operates at 10G baud and
 365          * replicates data to achieve lower speeds. Hereby, in this
 366          * default configuration we need to advertise all supported
 367          * modes and not only the ones we want to use.
 368          */
 369
 370         /* SR_AN_ADV3 */
 371         adv = 0;
 372         if (phylink_test(xpcs->supported, 2500baseX_Full))
 373                 adv |= DW_C73_2500KX;
 374
 375         /* TODO: 5000baseKR */
 376
 377         ret = xpcs_write(xpcs, MDIO_MMD_AN, DW_SR_AN_ADV3, adv);
 378         if (ret < 0)
 379                 return ret;
 380
 381         /* SR_AN_ADV2 */
 382         adv = 0;
 383         if (phylink_test(xpcs->supported, 1000baseKX_Full))
 384                 adv |= DW_C73_1000KX;
 385         if (phylink_test(xpcs->supported, 10000baseKX4_Full))
 386                 adv |= DW_C73_10000KX4;
 387         if (phylink_test(xpcs->supported, 10000baseKR_Full))
 388                 adv |= DW_C73_10000KR;
 389
 390         ret = xpcs_write(xpcs, MDIO_MMD_AN, DW_SR_AN_ADV2, adv);
 391         if (ret < 0)
 392                 return ret;
 393
 394         /* SR_AN_ADV1 */
 395         adv = DW_C73_AN_ADV_SF;
 396         if (phylink_test(xpcs->supported, Pause))
 397                 adv |= DW_C73_PAUSE;
 398         if (phylink_test(xpcs->supported, Asym_Pause))
 399                 adv |= DW_C73_ASYM_PAUSE;
 400
 401         return xpcs_write(xpcs, MDIO_MMD_AN, DW_SR_AN_ADV1, adv);
 402 }
 403
 404 static int xpcs_config_aneg(struct mdio_xpcs_args *xpcs)
 405 {
 406         int ret;
 407
 408         ret = xpcs_config_aneg_c73(xpcs);
 409         if (ret < 0)
 410                 return ret;
 411
 412         ret = xpcs_read(xpcs, MDIO_MMD_AN, MDIO_CTRL1);
 413         if (ret < 0)
 414                 return ret;
 415
 416         ret |= MDIO_AN_CTRL1_ENABLE | MDIO_AN_CTRL1_RESTART;
 417
 418         return xpcs_write(xpcs, MDIO_MMD_AN, MDIO_CTRL1, ret);
 419 }
 420
 421 static int xpcs_aneg_done(struct mdio_xpcs_args *xpcs,
 422                           struct phylink_link_state *state)
 423 {
 424         int ret;
 425
 426         ret = xpcs_read(xpcs, MDIO_MMD_AN, MDIO_STAT1);
 427         if (ret < 0)
 428                 return ret;
 429
 430         if (ret & MDIO_AN_STAT1_COMPLETE) {
 431                 ret = xpcs_read(xpcs, MDIO_MMD_AN, DW_SR_AN_LP_ABL1);
 432                 if (ret < 0)
 433                         return ret;
 434
 435                 /* Check if Aneg outcome is valid */
 436                 if (!(ret & DW_C73_AN_ADV_SF)) {
 437                         xpcs_config_aneg(xpcs);
 438                         return 0;
 439                 }
 440
 441                 return 1;
 442         }
 443
 444         return 0;
 445 }
 446
 447 static int xpcs_read_lpa(struct mdio_xpcs_args *xpcs,
 448                          struct phylink_link_state *state)
 449 {
 450         int ret;
 451
 452         ret = xpcs_read(xpcs, MDIO_MMD_AN, MDIO_STAT1);
 453         if (ret < 0)
 454                 return ret;
 455
 456         if (!(ret & MDIO_AN_STAT1_LPABLE)) {
 457                 phylink_clear(state->lp_advertising, Autoneg);
 458                 return 0;
 459         }
 460
 461         phylink_set(state->lp_advertising, Autoneg);
 462
 463         /* Clause 73 outcome */
 464         ret = xpcs_read(xpcs, MDIO_MMD_AN, DW_SR_AN_LP_ABL3);
 465         if (ret < 0)
 466                 return ret;
 467
 468         if (ret & DW_C73_2500KX)
 469                 phylink_set(state->lp_advertising, 2500baseX_Full);
 470
 471         ret = xpcs_read(xpcs, MDIO_MMD_AN, DW_SR_AN_LP_ABL2);
 472         if (ret < 0)
 473                 return ret;
 474
 475         if (ret & DW_C73_1000KX)
 476                 phylink_set(state->lp_advertising, 1000baseKX_Full);
 477         if (ret & DW_C73_10000KX4)
 478                 phylink_set(state->lp_advertising, 10000baseKX4_Full);
 479         if (ret & DW_C73_10000KR)
 480                 phylink_set(state->lp_advertising, 10000baseKR_Full);
 481
 482         ret = xpcs_read(xpcs, MDIO_MMD_AN, DW_SR_AN_LP_ABL1);
 483         if (ret < 0)
 484                 return ret;
 485
 486         if (ret & DW_C73_PAUSE)
 487                 phylink_set(state->lp_advertising, Pause);
 488         if (ret & DW_C73_ASYM_PAUSE)
 489                 phylink_set(state->lp_advertising, Asym_Pause);
 490
 491         linkmode_and(state->lp_advertising, state->lp_advertising,
 492                      state->advertising);
 493         return 0;
 494 }
 495
 496 static void xpcs_resolve_lpa(struct mdio_xpcs_args *xpcs,
 497                              struct phylink_link_state *state)
 498 {
 499         int max_speed = xpcs_get_max_usxgmii_speed(state->lp_advertising);
 500
 501         state->pause = MLO_PAUSE_TX | MLO_PAUSE_RX;
 502         state->speed = max_speed;
 503         state->duplex = DUPLEX_FULL;
 504 }
 505
 506 static int xpcs_get_max_xlgmii_speed(struct mdio_xpcs_args *xpcs,
 507                                      struct phylink_link_state *state)
 508 {
 509         unsigned long *adv = state->advertising;
 510         int speed = SPEED_UNKNOWN;
 511         int bit;
 512
 513         for_each_set_bit(bit, adv, __ETHTOOL_LINK_MODE_MASK_NBITS) {
 514                 int new_speed = SPEED_UNKNOWN;
 515
 516                 switch (bit) {
 517                 case ETHTOOL_LINK_MODE_25000baseCR_Full_BIT:
 518                 case ETHTOOL_LINK_MODE_25000baseKR_Full_BIT:
 519                 case ETHTOOL_LINK_MODE_25000baseSR_Full_BIT:
 520                         new_speed = SPEED_25000;
 521                         break;
 522                 case ETHTOOL_LINK_MODE_40000baseKR4_Full_BIT:
 523                 case ETHTOOL_LINK_MODE_40000baseCR4_Full_BIT:
 524                 case ETHTOOL_LINK_MODE_40000baseSR4_Full_BIT:
 525                 case ETHTOOL_LINK_MODE_40000baseLR4_Full_BIT:
 526                         new_speed = SPEED_40000;
 527                         break;
 528                 case ETHTOOL_LINK_MODE_50000baseCR2_Full_BIT:
 529                 case ETHTOOL_LINK_MODE_50000baseKR2_Full_BIT:
 530                 case ETHTOOL_LINK_MODE_50000baseSR2_Full_BIT:
 531                 case ETHTOOL_LINK_MODE_50000baseKR_Full_BIT:
 532                 case ETHTOOL_LINK_MODE_50000baseSR_Full_BIT:
 533                 case ETHTOOL_LINK_MODE_50000baseCR_Full_BIT:
 534                 case ETHTOOL_LINK_MODE_50000baseLR_ER_FR_Full_BIT:
 535                 case ETHTOOL_LINK_MODE_50000baseDR_Full_BIT:
 536                         new_speed = SPEED_50000;
 537                         break;
 538                 case ETHTOOL_LINK_MODE_100000baseKR4_Full_BIT:
 539                 case ETHTOOL_LINK_MODE_100000baseSR4_Full_BIT:
 540                 case ETHTOOL_LINK_MODE_100000baseCR4_Full_BIT:
 541                 case ETHTOOL_LINK_MODE_100000baseLR4_ER4_Full_BIT:
 542                 case ETHTOOL_LINK_MODE_100000baseKR2_Full_BIT:
 543                 case ETHTOOL_LINK_MODE_100000baseSR2_Full_BIT:
 544                 case ETHTOOL_LINK_MODE_100000baseCR2_Full_BIT:
 545                 case ETHTOOL_LINK_MODE_100000baseLR2_ER2_FR2_Full_BIT:
 546                 case ETHTOOL_LINK_MODE_100000baseDR2_Full_BIT:
 547                         new_speed = SPEED_100000;
 548                         break;
 549                 default:
 550                         continue;
 551                 }
 552
 553                 if (new_speed > speed)
 554                         speed = new_speed;
 555         }
 556
 557         return speed;
 558 }
 559
 560 static void xpcs_resolve_pma(struct mdio_xpcs_args *xpcs,
 561                              struct phylink_link_state *state)
 562 {
 563         state->pause = MLO_PAUSE_TX | MLO_PAUSE_RX;
 564         state->duplex = DUPLEX_FULL;
 565
 566         switch (state->interface) {
 567         case PHY_INTERFACE_MODE_10GKR:
 568                 state->speed = SPEED_10000;
 569                 break;
 570         case PHY_INTERFACE_MODE_XLGMII:
 571                 state->speed = xpcs_get_max_xlgmii_speed(xpcs, state);
 572                 break;
 573         default:
 574                 state->speed = SPEED_UNKNOWN;
 575                 break;
 576         }
 577 }
 578
 579 static int xpcs_validate(struct mdio_xpcs_args *xpcs,
 580                          unsigned long *supported,
 581                          struct phylink_link_state *state)
 582 {
 583         linkmode_and(supported, supported, xpcs->supported);
 584         linkmode_and(state->advertising, state->advertising, xpcs->supported);
 585         return 0;
 586 }
 587
 588 static int xpcs_config(struct mdio_xpcs_args *xpcs,
 589                        const struct phylink_link_state *state)
 590 {
 591         int ret;
 592
 593         if (state->an_enabled) {
 594                 ret = xpcs_config_aneg(xpcs);
 595                 if (ret)
 596                         return ret;
 597         }
 598
 599         return 0;
 600 }
 601
 602 static int xpcs_get_state(struct mdio_xpcs_args *xpcs,
 603                           struct phylink_link_state *state)
 604 {
 605         int ret;
 606
 607         /* Link needs to be read first ... */
 608         state->link = xpcs_read_link(xpcs, state->an_enabled) > 0 ? 1 : 0;
 609
 610         /* ... and then we check the faults. */
 611         ret = xpcs_read_fault(xpcs, state);
 612         if (ret) {
 613                 ret = xpcs_soft_reset(xpcs, MDIO_MMD_PCS);
 614                 if (ret)
 615                         return ret;
 616
 617                 state->link = 0;
 618
 619                 return xpcs_config(xpcs, state);
 620         }
 621
 622         if (state->an_enabled && xpcs_aneg_done(xpcs, state)) {
 623                 state->an_complete = true;
 624                 xpcs_read_lpa(xpcs, state);
 625                 xpcs_resolve_lpa(xpcs, state);
 626         } else if (state->an_enabled) {
 627                 state->link = 0;
 628         } else if (state->link) {
 629                 xpcs_resolve_pma(xpcs, state);
 630         }
 631
 632         return 0;
 633 }
 634
 635 static int xpcs_link_up(struct mdio_xpcs_args *xpcs, int speed,
 636                         phy_interface_t interface)
 637 {
 638         if (interface == PHY_INTERFACE_MODE_USXGMII)
 639                 return xpcs_config_usxgmii(xpcs, speed);
 640
 641         return 0;
 642 }
 643
 644 static u32 xpcs_get_id(struct mdio_xpcs_args *xpcs)
 645 {
 646         int ret;
 647         u32 id;
 648
 649         ret = xpcs_read(xpcs, MDIO_MMD_PCS, MII_PHYSID1);
 650         if (ret < 0)
 651                 return 0xffffffff;
 652
 653         id = ret << 16;
 654
 655         ret = xpcs_read(xpcs, MDIO_MMD_PCS, MII_PHYSID2);
 656         if (ret < 0)
 657                 return 0xffffffff;
 658
 659         return id | ret;
 660 }
 661
 662 static bool xpcs_check_features(struct mdio_xpcs_args *xpcs,
 663                                 struct xpcs_id *match,
 664                                 phy_interface_t interface)
 665 {
 666         int i;
 667
 668         for (i = 0; match->interface[i] != PHY_INTERFACE_MODE_MAX; i++) {
 669                 if (match->interface[i] == interface)
 670                         break;
 671         }
 672
 673         if (match->interface[i] == PHY_INTERFACE_MODE_MAX)
 674                 return false;
 675
 676         for (i = 0; match->supported[i] != __ETHTOOL_LINK_MODE_MASK_NBITS; i++)
 677                 set_bit(match->supported[i], xpcs->supported);
 678
 679         return true;
 680 }
 681
 682 static int xpcs_probe(struct mdio_xpcs_args *xpcs, phy_interface_t interface)
 683 {
 684         u32 xpcs_id = xpcs_get_id(xpcs);
 685         struct xpcs_id *match = NULL;
 686         int i;
 687
 688         for (i = 0; i < ARRAY_SIZE(xpcs_id_list); i++) {
 689                 struct xpcs_id *entry = &xpcs_id_list[i];
 690
 691                 if ((xpcs_id & entry->mask) == entry->id) {
 692                         match = entry;
 693
 694                         if (xpcs_check_features(xpcs, match, interface))
 695                                 return xpcs_soft_reset(xpcs, MDIO_MMD_PCS);
 696                 }
 697         }
 698
 699         return -ENODEV;
 700 }
 701
 702 static struct mdio_xpcs_ops xpcs_ops = {
 703         .validate = xpcs_validate,
 704         .config = xpcs_config,
 705         .get_state = xpcs_get_state,
 706         .link_up = xpcs_link_up,
 707         .probe = xpcs_probe,
 708 };
 709
 710 struct mdio_xpcs_ops *mdio_xpcs_get_ops(void)
 711 {
 712         return &xpcs_ops;
 713 }
 714 EXPORT_SYMBOL_GPL(mdio_xpcs_get_ops);
 715
 716 MODULE_LICENSE("GPL v2");