drivers/net/phy/phy-core.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * Core PHY library, taken from phy.c
   4  */
   5 #include <linux/export.h>
   6 #include <linux/phy.h>
   7 #include <linux/of.h>
   8
   9 const char *phy_speed_to_str(int speed)
  10 {
  11         BUILD_BUG_ON_MSG(__ETHTOOL_LINK_MODE_MASK_NBITS != 75,
  12                 "Enum ethtool_link_mode_bit_indices and phylib are out of sync. "
  13                 "If a speed or mode has been added please update phy_speed_to_str "
  14                 "and the PHY settings array.\n");
  15
  16         switch (speed) {
  17         case SPEED_10:
  18                 return "10Mbps";
  19         case SPEED_100:
  20                 return "100Mbps";
  21         case SPEED_1000:
  22                 return "1Gbps";
  23         case SPEED_2500:
  24                 return "2.5Gbps";
  25         case SPEED_5000:
  26                 return "5Gbps";
  27         case SPEED_10000:
  28                 return "10Gbps";
  29         case SPEED_14000:
  30                 return "14Gbps";
  31         case SPEED_20000:
  32                 return "20Gbps";
  33         case SPEED_25000:
  34                 return "25Gbps";
  35         case SPEED_40000:
  36                 return "40Gbps";
  37         case SPEED_50000:
  38                 return "50Gbps";
  39         case SPEED_56000:
  40                 return "56Gbps";
  41         case SPEED_100000:
  42                 return "100Gbps";
  43         case SPEED_200000:
  44                 return "200Gbps";
  45         case SPEED_400000:
  46                 return "400Gbps";
  47         case SPEED_UNKNOWN:
  48                 return "Unknown";
  49         default:
  50                 return "Unsupported (update phy-core.c)";
  51         }
  52 }
  53 EXPORT_SYMBOL_GPL(phy_speed_to_str);
  54
  55 const char *phy_duplex_to_str(unsigned int duplex)
  56 {
  57         if (duplex == DUPLEX_HALF)
  58                 return "Half";
  59         if (duplex == DUPLEX_FULL)
  60                 return "Full";
  61         if (duplex == DUPLEX_UNKNOWN)
  62                 return "Unknown";
  63         return "Unsupported (update phy-core.c)";
  64 }
  65 EXPORT_SYMBOL_GPL(phy_duplex_to_str);
  66
  67 /* A mapping of all SUPPORTED settings to speed/duplex.  This table
  68  * must be grouped by speed and sorted in descending match priority
  69  * - iow, descending speed. */
  70
  71 #define PHY_SETTING(s, d, b) { .speed = SPEED_ ## s, .duplex = DUPLEX_ ## d, \
  72                                .bit = ETHTOOL_LINK_MODE_ ## b ## _BIT}
  73
  74 static const struct phy_setting settings[] = {
  75         /* 400G */
  76         PHY_SETTING( 400000, FULL, 400000baseCR8_Full           ),
  77         PHY_SETTING( 400000, FULL, 400000baseKR8_Full           ),
  78         PHY_SETTING( 400000, FULL, 400000baseLR8_ER8_FR8_Full   ),
  79         PHY_SETTING( 400000, FULL, 400000baseDR8_Full           ),
  80         PHY_SETTING( 400000, FULL, 400000baseSR8_Full           ),
  81         /* 200G */
  82         PHY_SETTING( 200000, FULL, 200000baseCR4_Full           ),
  83         PHY_SETTING( 200000, FULL, 200000baseKR4_Full           ),
  84         PHY_SETTING( 200000, FULL, 200000baseLR4_ER4_FR4_Full   ),
  85         PHY_SETTING( 200000, FULL, 200000baseDR4_Full           ),
  86         PHY_SETTING( 200000, FULL, 200000baseSR4_Full           ),
  87         /* 100G */
  88         PHY_SETTING( 100000, FULL, 100000baseCR4_Full           ),
  89         PHY_SETTING( 100000, FULL, 100000baseKR4_Full           ),
  90         PHY_SETTING( 100000, FULL, 100000baseLR4_ER4_Full       ),
  91         PHY_SETTING( 100000, FULL, 100000baseSR4_Full           ),
  92         PHY_SETTING( 100000, FULL, 100000baseCR2_Full           ),
  93         PHY_SETTING( 100000, FULL, 100000baseKR2_Full           ),
  94         PHY_SETTING( 100000, FULL, 100000baseLR2_ER2_FR2_Full   ),
  95         PHY_SETTING( 100000, FULL, 100000baseDR2_Full           ),
  96         PHY_SETTING( 100000, FULL, 100000baseSR2_Full           ),
  97         /* 56G */
  98         PHY_SETTING(  56000, FULL,  56000baseCR4_Full           ),
  99         PHY_SETTING(  56000, FULL,  56000baseKR4_Full           ),
 100         PHY_SETTING(  56000, FULL,  56000baseLR4_Full           ),
 101         PHY_SETTING(  56000, FULL,  56000baseSR4_Full           ),
 102         /* 50G */
 103         PHY_SETTING(  50000, FULL,  50000baseCR2_Full           ),
 104         PHY_SETTING(  50000, FULL,  50000baseKR2_Full           ),
 105         PHY_SETTING(  50000, FULL,  50000baseSR2_Full           ),
 106         PHY_SETTING(  50000, FULL,  50000baseCR_Full            ),
 107         PHY_SETTING(  50000, FULL,  50000baseKR_Full            ),
 108         PHY_SETTING(  50000, FULL,  50000baseLR_ER_FR_Full      ),
 109         PHY_SETTING(  50000, FULL,  50000baseDR_Full            ),
 110         PHY_SETTING(  50000, FULL,  50000baseSR_Full            ),
 111         /* 40G */
 112         PHY_SETTING(  40000, FULL,  40000baseCR4_Full           ),
 113         PHY_SETTING(  40000, FULL,  40000baseKR4_Full           ),
 114         PHY_SETTING(  40000, FULL,  40000baseLR4_Full           ),
 115         PHY_SETTING(  40000, FULL,  40000baseSR4_Full           ),
 116         /* 25G */
 117         PHY_SETTING(  25000, FULL,  25000baseCR_Full            ),
 118         PHY_SETTING(  25000, FULL,  25000baseKR_Full            ),
 119         PHY_SETTING(  25000, FULL,  25000baseSR_Full            ),
 120         /* 20G */
 121         PHY_SETTING(  20000, FULL,  20000baseKR2_Full           ),
 122         PHY_SETTING(  20000, FULL,  20000baseMLD2_Full          ),
 123         /* 10G */
 124         PHY_SETTING(  10000, FULL,  10000baseCR_Full            ),
 125         PHY_SETTING(  10000, FULL,  10000baseER_Full            ),
 126         PHY_SETTING(  10000, FULL,  10000baseKR_Full            ),
 127         PHY_SETTING(  10000, FULL,  10000baseKX4_Full           ),
 128         PHY_SETTING(  10000, FULL,  10000baseLR_Full            ),
 129         PHY_SETTING(  10000, FULL,  10000baseLRM_Full           ),
 130         PHY_SETTING(  10000, FULL,  10000baseR_FEC              ),
 131         PHY_SETTING(  10000, FULL,  10000baseSR_Full            ),
 132         PHY_SETTING(  10000, FULL,  10000baseT_Full             ),
 133         /* 5G */
 134         PHY_SETTING(   5000, FULL,   5000baseT_Full             ),
 135         /* 2.5G */
 136         PHY_SETTING(   2500, FULL,   2500baseT_Full             ),
 137         PHY_SETTING(   2500, FULL,   2500baseX_Full             ),
 138         /* 1G */
 139         PHY_SETTING(   1000, FULL,   1000baseKX_Full            ),
 140         PHY_SETTING(   1000, FULL,   1000baseT_Full             ),
 141         PHY_SETTING(   1000, HALF,   1000baseT_Half             ),
 142         PHY_SETTING(   1000, FULL,   1000baseT1_Full            ),
 143         PHY_SETTING(   1000, FULL,   1000baseX_Full             ),
 144         /* 100M */
 145         PHY_SETTING(    100, FULL,    100baseT_Full             ),
 146         PHY_SETTING(    100, FULL,    100baseT1_Full            ),
 147         PHY_SETTING(    100, HALF,    100baseT_Half             ),
 148         /* 10M */
 149         PHY_SETTING(     10, FULL,     10baseT_Full             ),
 150         PHY_SETTING(     10, HALF,     10baseT_Half             ),
 151 };
 152 #undef PHY_SETTING
 153
 154 /**
 155  * phy_lookup_setting - lookup a PHY setting
 156  * @speed: speed to match
 157  * @duplex: duplex to match
 158  * @mask: allowed link modes
 159  * @exact: an exact match is required
 160  *
 161  * Search the settings array for a setting that matches the speed and
 162  * duplex, and which is supported.
 163  *
 164  * If @exact is unset, either an exact match or %NULL for no match will
 165  * be returned.
 166  *
 167  * If @exact is set, an exact match, the fastest supported setting at
 168  * or below the specified speed, the slowest supported setting, or if
 169  * they all fail, %NULL will be returned.
 170  */
 171 const struct phy_setting *
 172 phy_lookup_setting(int speed, int duplex, const unsigned long *mask, bool exact)
 173 {
 174         const struct phy_setting *p, *match = NULL, *last = NULL;
 175         int i;
 176
 177         for (i = 0, p = settings; i < ARRAY_SIZE(settings); i++, p++) {
 178                 if (p->bit < __ETHTOOL_LINK_MODE_MASK_NBITS &&
 179                     test_bit(p->bit, mask)) {
 180                         last = p;
 181                         if (p->speed == speed && p->duplex == duplex) {
 182                                 /* Exact match for speed and duplex */
 183                                 match = p;
 184                                 break;
 185                         } else if (!exact) {
 186                                 if (!match && p->speed <= speed)
 187                                         /* Candidate */
 188                                         match = p;
 189
 190                                 if (p->speed < speed)
 191                                         break;
 192                         }
 193                 }
 194         }
 195
 196         if (!match && !exact)
 197                 match = last;
 198
 199         return match;
 200 }
 201 EXPORT_SYMBOL_GPL(phy_lookup_setting);
 202
 203 size_t phy_speeds(unsigned int *speeds, size_t size,
 204                   unsigned long *mask)
 205 {
 206         size_t count;
 207         int i;
 208
 209         for (i = 0, count = 0; i < ARRAY_SIZE(settings) && count < size; i++)
 210                 if (settings[i].bit < __ETHTOOL_LINK_MODE_MASK_NBITS &&
 211                     test_bit(settings[i].bit, mask) &&
 212                     (count == 0 || speeds[count - 1] != settings[i].speed))
 213                         speeds[count++] = settings[i].speed;
 214
 215         return count;
 216 }
 217
 218 static int __set_linkmode_max_speed(u32 max_speed, unsigned long *addr)
 219 {
 220         const struct phy_setting *p;
 221         int i;
 222
 223         for (i = 0, p = settings; i < ARRAY_SIZE(settings); i++, p++) {
 224                 if (p->speed > max_speed)
 225                         linkmode_clear_bit(p->bit, addr);
 226                 else
 227                         break;
 228         }
 229
 230         return 0;
 231 }
 232
 233 static int __set_phy_supported(struct phy_device *phydev, u32 max_speed)
 234 {
 235         return __set_linkmode_max_speed(max_speed, phydev->supported);
 236 }
 237
 238 int phy_set_max_speed(struct phy_device *phydev, u32 max_speed)
 239 {
 240         int err;
 241
 242         err = __set_phy_supported(phydev, max_speed);
 243         if (err)
 244                 return err;
 245
 246         phy_advertise_supported(phydev);
 247
 248         return 0;
 249 }
 250 EXPORT_SYMBOL(phy_set_max_speed);
 251
 252 void of_set_phy_supported(struct phy_device *phydev)
 253 {
 254         struct device_node *node = phydev->mdio.dev.of_node;
 255         u32 max_speed;
 256
 257         if (!IS_ENABLED(CONFIG_OF_MDIO))
 258                 return;
 259
 260         if (!node)
 261                 return;
 262
 263         if (!of_property_read_u32(node, "max-speed", &max_speed))
 264                 __set_phy_supported(phydev, max_speed);
 265 }
 266
 267 void of_set_phy_eee_broken(struct phy_device *phydev)
 268 {
 269         struct device_node *node = phydev->mdio.dev.of_node;
 270         u32 broken = 0;
 271
 272         if (!IS_ENABLED(CONFIG_OF_MDIO))
 273                 return;
 274
 275         if (!node)
 276                 return;
 277
 278         if (of_property_read_bool(node, "eee-broken-100tx"))
 279                 broken |= MDIO_EEE_100TX;
 280         if (of_property_read_bool(node, "eee-broken-1000t"))
 281                 broken |= MDIO_EEE_1000T;
 282         if (of_property_read_bool(node, "eee-broken-10gt"))
 283                 broken |= MDIO_EEE_10GT;
 284         if (of_property_read_bool(node, "eee-broken-1000kx"))
 285                 broken |= MDIO_EEE_1000KX;
 286         if (of_property_read_bool(node, "eee-broken-10gkx4"))
 287                 broken |= MDIO_EEE_10GKX4;
 288         if (of_property_read_bool(node, "eee-broken-10gkr"))
 289                 broken |= MDIO_EEE_10GKR;
 290
 291         phydev->eee_broken_modes = broken;
 292 }
 293
 294 void phy_resolve_aneg_pause(struct phy_device *phydev)
 295 {
 296         if (phydev->duplex == DUPLEX_FULL) {
 297                 phydev->pause = linkmode_test_bit(ETHTOOL_LINK_MODE_Pause_BIT,
 298                                                   phydev->lp_advertising);
 299                 phydev->asym_pause = linkmode_test_bit(
 300                         ETHTOOL_LINK_MODE_Asym_Pause_BIT,
 301                         phydev->lp_advertising);
 302         }
 303 }
 304 EXPORT_SYMBOL_GPL(phy_resolve_aneg_pause);
 305
 306 /**
 307  * phy_resolve_aneg_linkmode - resolve the advertisements into phy settings
 308  * @phydev: The phy_device struct
 309  *
 310  * Resolve our and the link partner advertisements into their corresponding
 311  * speed and duplex. If full duplex was negotiated, extract the pause mode
 312  * from the link partner mask.
 313  */
 314 void phy_resolve_aneg_linkmode(struct phy_device *phydev)
 315 {
 316         __ETHTOOL_DECLARE_LINK_MODE_MASK(common);
 317         int i;
 318
 319         linkmode_and(common, phydev->lp_advertising, phydev->advertising);
 320
 321         for (i = 0; i < ARRAY_SIZE(settings); i++)
 322                 if (test_bit(settings[i].bit, common)) {
 323                         phydev->speed = settings[i].speed;
 324                         phydev->duplex = settings[i].duplex;
 325                         break;
 326                 }
 327
 328         phy_resolve_aneg_pause(phydev);
 329 }
 330 EXPORT_SYMBOL_GPL(phy_resolve_aneg_linkmode);
 331
 332 /**
 333  * phy_check_downshift - check whether downshift occurred
 334  * @phydev: The phy_device struct
 335  *
 336  * Check whether a downshift to a lower speed occurred. If this should be the
 337  * case warn the user.
 338  * Prerequisite for detecting downshift is that PHY driver implements the
 339  * read_status callback and sets phydev->speed to the actual link speed.
 340  */
 341 void phy_check_downshift(struct phy_device *phydev)
 342 {
 343         __ETHTOOL_DECLARE_LINK_MODE_MASK(common);
 344         int i, speed = SPEED_UNKNOWN;
 345
 346         phydev->downshifted_rate = 0;
 347
 348         if (phydev->autoneg == AUTONEG_DISABLE ||
 349             phydev->speed == SPEED_UNKNOWN)
 350                 return;
 351
 352         linkmode_and(common, phydev->lp_advertising, phydev->advertising);
 353
 354         for (i = 0; i < ARRAY_SIZE(settings); i++)
 355                 if (test_bit(settings[i].bit, common)) {
 356                         speed = settings[i].speed;
 357                         break;
 358                 }
 359
 360         if (speed == SPEED_UNKNOWN || phydev->speed >= speed)
 361                 return;
 362
 363         phydev_warn(phydev, "Downshift occurred from negotiated speed %s to actual speed %s, check cabling!\n",
 364                     phy_speed_to_str(speed), phy_speed_to_str(phydev->speed));
 365
 366         phydev->downshifted_rate = 1;
 367 }
 368 EXPORT_SYMBOL_GPL(phy_check_downshift);
 369
 370 static int phy_resolve_min_speed(struct phy_device *phydev, bool fdx_only)
 371 {
 372         __ETHTOOL_DECLARE_LINK_MODE_MASK(common);
 373         int i = ARRAY_SIZE(settings);
 374
 375         linkmode_and(common, phydev->lp_advertising, phydev->advertising);
 376
 377         while (--i >= 0) {
 378                 if (test_bit(settings[i].bit, common)) {
 379                         if (fdx_only && settings[i].duplex != DUPLEX_FULL)
 380                                 continue;
 381                         return settings[i].speed;
 382                 }
 383         }
 384
 385         return SPEED_UNKNOWN;
 386 }
 387
 388 int phy_speed_down_core(struct phy_device *phydev)
 389 {
 390         int min_common_speed = phy_resolve_min_speed(phydev, true);
 391
 392         if (min_common_speed == SPEED_UNKNOWN)
 393                 return -EINVAL;
 394
 395         return __set_linkmode_max_speed(min_common_speed, phydev->advertising);
 396 }
 397
 398 static void mmd_phy_indirect(struct mii_bus *bus, int phy_addr, int devad,
 399                              u16 regnum)
 400 {
 401         /* Write the desired MMD Devad */
 402         __mdiobus_write(bus, phy_addr, MII_MMD_CTRL, devad);
 403
 404         /* Write the desired MMD register address */
 405         __mdiobus_write(bus, phy_addr, MII_MMD_DATA, regnum);
 406
 407         /* Select the Function : DATA with no post increment */
 408         __mdiobus_write(bus, phy_addr, MII_MMD_CTRL,
 409                         devad | MII_MMD_CTRL_NOINCR);
 410 }
 411
 412 /**
 413  * __phy_read_mmd - Convenience function for reading a register
 414  * from an MMD on a given PHY.
 415  * @phydev: The phy_device struct
 416  * @devad: The MMD to read from (0..31)
 417  * @regnum: The register on the MMD to read (0..65535)
 418  *
 419  * Same rules as for __phy_read();
 420  */
 421 int __phy_read_mmd(struct phy_device *phydev, int devad, u32 regnum)
 422 {
 423         int val;
 424
 425         if (regnum > (u16)~0 || devad > 32)
 426                 return -EINVAL;
 427
 428         if (phydev->drv && phydev->drv->read_mmd) {
 429                 val = phydev->drv->read_mmd(phydev, devad, regnum);
 430         } else if (phydev->is_c45) {
 431                 val = __mdiobus_c45_read(phydev->mdio.bus, phydev->mdio.addr,
 432                                          devad, regnum);
 433         } else {
 434                 struct mii_bus *bus = phydev->mdio.bus;
 435                 int phy_addr = phydev->mdio.addr;
 436
 437                 mmd_phy_indirect(bus, phy_addr, devad, regnum);
 438
 439                 /* Read the content of the MMD's selected register */
 440                 val = __mdiobus_read(bus, phy_addr, MII_MMD_DATA);
 441         }
 442         return val;
 443 }
 444 EXPORT_SYMBOL(__phy_read_mmd);
 445
 446 /**
 447  * phy_read_mmd - Convenience function for reading a register
 448  * from an MMD on a given PHY.
 449  * @phydev: The phy_device struct
 450  * @devad: The MMD to read from
 451  * @regnum: The register on the MMD to read
 452  *
 453  * Same rules as for phy_read();
 454  */
 455 int phy_read_mmd(struct phy_device *phydev, int devad, u32 regnum)
 456 {
 457         int ret;
 458
 459         phy_lock_mdio_bus(phydev);
 460         ret = __phy_read_mmd(phydev, devad, regnum);
 461         phy_unlock_mdio_bus(phydev);
 462
 463         return ret;
 464 }
 465 EXPORT_SYMBOL(phy_read_mmd);
 466
 467 /**
 468  * __phy_write_mmd - Convenience function for writing a register
 469  * on an MMD on a given PHY.
 470  * @phydev: The phy_device struct
 471  * @devad: The MMD to read from
 472  * @regnum: The register on the MMD to read
 473  * @val: value to write to @regnum
 474  *
 475  * Same rules as for __phy_write();
 476  */
 477 int __phy_write_mmd(struct phy_device *phydev, int devad, u32 regnum, u16 val)
 478 {
 479         int ret;
 480
 481         if (regnum > (u16)~0 || devad > 32)
 482                 return -EINVAL;
 483
 484         if (phydev->drv && phydev->drv->write_mmd) {
 485                 ret = phydev->drv->write_mmd(phydev, devad, regnum, val);
 486         } else if (phydev->is_c45) {
 487                 ret = __mdiobus_c45_write(phydev->mdio.bus, phydev->mdio.addr,
 488                                           devad, regnum, val);
 489         } else {
 490                 struct mii_bus *bus = phydev->mdio.bus;
 491                 int phy_addr = phydev->mdio.addr;
 492
 493                 mmd_phy_indirect(bus, phy_addr, devad, regnum);
 494
 495                 /* Write the data into MMD's selected register */
 496                 __mdiobus_write(bus, phy_addr, MII_MMD_DATA, val);
 497
 498                 ret = 0;
 499         }
 500         return ret;
 501 }
 502 EXPORT_SYMBOL(__phy_write_mmd);
 503
 504 /**
 505  * phy_write_mmd - Convenience function for writing a register
 506  * on an MMD on a given PHY.
 507  * @phydev: The phy_device struct
 508  * @devad: The MMD to read from
 509  * @regnum: The register on the MMD to read
 510  * @val: value to write to @regnum
 511  *
 512  * Same rules as for phy_write();
 513  */
 514 int phy_write_mmd(struct phy_device *phydev, int devad, u32 regnum, u16 val)
 515 {
 516         int ret;
 517
 518         phy_lock_mdio_bus(phydev);
 519         ret = __phy_write_mmd(phydev, devad, regnum, val);
 520         phy_unlock_mdio_bus(phydev);
 521
 522         return ret;
 523 }
 524 EXPORT_SYMBOL(phy_write_mmd);
 525
 526 /**
 527  * phy_modify_changed - Function for modifying a PHY register
 528  * @phydev: the phy_device struct
 529  * @regnum: register number to modify
 530  * @mask: bit mask of bits to clear
 531  * @set: new value of bits set in mask to write to @regnum
 532  *
 533  * NOTE: MUST NOT be called from interrupt context,
 534  * because the bus read/write functions may wait for an interrupt
 535  * to conclude the operation.
 536  *
 537  * Returns negative errno, 0 if there was no change, and 1 in case of change
 538  */
 539 int phy_modify_changed(struct phy_device *phydev, u32 regnum, u16 mask, u16 set)
 540 {
 541         int ret;
 542
 543         phy_lock_mdio_bus(phydev);
 544         ret = __phy_modify_changed(phydev, regnum, mask, set);
 545         phy_unlock_mdio_bus(phydev);
 546
 547         return ret;
 548 }
 549 EXPORT_SYMBOL_GPL(phy_modify_changed);
 550
 551 /**
 552  * __phy_modify - Convenience function for modifying a PHY register
 553  * @phydev: the phy_device struct
 554  * @regnum: register number to modify
 555  * @mask: bit mask of bits to clear
 556  * @set: new value of bits set in mask to write to @regnum
 557  *
 558  * NOTE: MUST NOT be called from interrupt context,
 559  * because the bus read/write functions may wait for an interrupt
 560  * to conclude the operation.
 561  */
 562 int __phy_modify(struct phy_device *phydev, u32 regnum, u16 mask, u16 set)
 563 {
 564         int ret;
 565
 566         ret = __phy_modify_changed(phydev, regnum, mask, set);
 567
 568         return ret < 0 ? ret : 0;
 569 }
 570 EXPORT_SYMBOL_GPL(__phy_modify);
 571
 572 /**
 573  * phy_modify - Convenience function for modifying a given PHY register
 574  * @phydev: the phy_device struct
 575  * @regnum: register number to write
 576  * @mask: bit mask of bits to clear
 577  * @set: new value of bits set in mask to write to @regnum
 578  *
 579  * NOTE: MUST NOT be called from interrupt context,
 580  * because the bus read/write functions may wait for an interrupt
 581  * to conclude the operation.
 582  */
 583 int phy_modify(struct phy_device *phydev, u32 regnum, u16 mask, u16 set)
 584 {
 585         int ret;
 586
 587         phy_lock_mdio_bus(phydev);
 588         ret = __phy_modify(phydev, regnum, mask, set);
 589         phy_unlock_mdio_bus(phydev);
 590
 591         return ret;
 592 }
 593 EXPORT_SYMBOL_GPL(phy_modify);
 594
 595 /**
 596  * __phy_modify_mmd_changed - Function for modifying a register on MMD
 597  * @phydev: the phy_device struct
 598  * @devad: the MMD containing register to modify
 599  * @regnum: register number to modify
 600  * @mask: bit mask of bits to clear
 601  * @set: new value of bits set in mask to write to @regnum
 602  *
 603  * Unlocked helper function which allows a MMD register to be modified as
 604  * new register value = (old register value & ~mask) | set
 605  *
 606  * Returns negative errno, 0 if there was no change, and 1 in case of change
 607  */
 608 int __phy_modify_mmd_changed(struct phy_device *phydev, int devad, u32 regnum,
 609                              u16 mask, u16 set)
 610 {
 611         int new, ret;
 612
 613         ret = __phy_read_mmd(phydev, devad, regnum);
 614         if (ret < 0)
 615                 return ret;
 616
 617         new = (ret & ~mask) | set;
 618         if (new == ret)
 619                 return 0;
 620
 621         ret = __phy_write_mmd(phydev, devad, regnum, new);
 622
 623         return ret < 0 ? ret : 1;
 624 }
 625 EXPORT_SYMBOL_GPL(__phy_modify_mmd_changed);
 626
 627 /**
 628  * phy_modify_mmd_changed - Function for modifying a register on MMD
 629  * @phydev: the phy_device struct
 630  * @devad: the MMD containing register to modify
 631  * @regnum: register number to modify
 632  * @mask: bit mask of bits to clear
 633  * @set: new value of bits set in mask to write to @regnum
 634  *
 635  * NOTE: MUST NOT be called from interrupt context,
 636  * because the bus read/write functions may wait for an interrupt
 637  * to conclude the operation.
 638  *
 639  * Returns negative errno, 0 if there was no change, and 1 in case of change
 640  */
 641 int phy_modify_mmd_changed(struct phy_device *phydev, int devad, u32 regnum,
 642                            u16 mask, u16 set)
 643 {
 644         int ret;
 645
 646         phy_lock_mdio_bus(phydev);
 647         ret = __phy_modify_mmd_changed(phydev, devad, regnum, mask, set);
 648         phy_unlock_mdio_bus(phydev);
 649
 650         return ret;
 651 }
 652 EXPORT_SYMBOL_GPL(phy_modify_mmd_changed);
 653
 654 /**
 655  * __phy_modify_mmd - Convenience function for modifying a register on MMD
 656  * @phydev: the phy_device struct
 657  * @devad: the MMD containing register to modify
 658  * @regnum: register number to modify
 659  * @mask: bit mask of bits to clear
 660  * @set: new value of bits set in mask to write to @regnum
 661  *
 662  * NOTE: MUST NOT be called from interrupt context,
 663  * because the bus read/write functions may wait for an interrupt
 664  * to conclude the operation.
 665  */
 666 int __phy_modify_mmd(struct phy_device *phydev, int devad, u32 regnum,
 667                      u16 mask, u16 set)
 668 {
 669         int ret;
 670
 671         ret = __phy_modify_mmd_changed(phydev, devad, regnum, mask, set);
 672
 673         return ret < 0 ? ret : 0;
 674 }
 675 EXPORT_SYMBOL_GPL(__phy_modify_mmd);
 676
 677 /**
 678  * phy_modify_mmd - Convenience function for modifying a register on MMD
 679  * @phydev: the phy_device struct
 680  * @devad: the MMD containing register to modify
 681  * @regnum: register number to modify
 682  * @mask: bit mask of bits to clear
 683  * @set: new value of bits set in mask to write to @regnum
 684  *
 685  * NOTE: MUST NOT be called from interrupt context,
 686  * because the bus read/write functions may wait for an interrupt
 687  * to conclude the operation.
 688  */
 689 int phy_modify_mmd(struct phy_device *phydev, int devad, u32 regnum,
 690                    u16 mask, u16 set)
 691 {
 692         int ret;
 693
 694         phy_lock_mdio_bus(phydev);
 695         ret = __phy_modify_mmd(phydev, devad, regnum, mask, set);
 696         phy_unlock_mdio_bus(phydev);
 697
 698         return ret;
 699 }
 700 EXPORT_SYMBOL_GPL(phy_modify_mmd);
 701
 702 static int __phy_read_page(struct phy_device *phydev)
 703 {
 704         if (WARN_ONCE(!phydev->drv->read_page, "read_page callback not available, PHY driver not loaded?\n"))
 705                 return -EOPNOTSUPP;
 706
 707         return phydev->drv->read_page(phydev);
 708 }
 709
 710 static int __phy_write_page(struct phy_device *phydev, int page)
 711 {
 712         if (WARN_ONCE(!phydev->drv->write_page, "write_page callback not available, PHY driver not loaded?\n"))
 713                 return -EOPNOTSUPP;
 714
 715         return phydev->drv->write_page(phydev, page);
 716 }
 717
 718 /**
 719  * phy_save_page() - take the bus lock and save the current page
 720  * @phydev: a pointer to a &struct phy_device
 721  *
 722  * Take the MDIO bus lock, and return the current page number. On error,
 723  * returns a negative errno. phy_restore_page() must always be called
 724  * after this, irrespective of success or failure of this call.
 725  */
 726 int phy_save_page(struct phy_device *phydev)
 727 {
 728         phy_lock_mdio_bus(phydev);
 729         return __phy_read_page(phydev);
 730 }
 731 EXPORT_SYMBOL_GPL(phy_save_page);
 732
 733 /**
 734  * phy_select_page() - take the bus lock, save the current page, and set a page
 735  * @phydev: a pointer to a &struct phy_device
 736  * @page: desired page
 737  *
 738  * Take the MDIO bus lock to protect against concurrent access, save the
 739  * current PHY page, and set the current page.  On error, returns a
 740  * negative errno, otherwise returns the previous page number.
 741  * phy_restore_page() must always be called after this, irrespective
 742  * of success or failure of this call.
 743  */
 744 int phy_select_page(struct phy_device *phydev, int page)
 745 {
 746         int ret, oldpage;
 747
 748         oldpage = ret = phy_save_page(phydev);
 749         if (ret < 0)
 750                 return ret;
 751
 752         if (oldpage != page) {
 753                 ret = __phy_write_page(phydev, page);
 754                 if (ret < 0)
 755                         return ret;
 756         }
 757
 758         return oldpage;
 759 }
 760 EXPORT_SYMBOL_GPL(phy_select_page);
 761
 762 /**
 763  * phy_restore_page() - restore the page register and release the bus lock
 764  * @phydev: a pointer to a &struct phy_device
 765  * @oldpage: the old page, return value from phy_save_page() or phy_select_page()
 766  * @ret: operation's return code
 767  *
 768  * Release the MDIO bus lock, restoring @oldpage if it is a valid page.
 769  * This function propagates the earliest error code from the group of
 770  * operations.
 771  *
 772  * Returns:
 773  *   @oldpage if it was a negative value, otherwise
 774  *   @ret if it was a negative errno value, otherwise
 775  *   phy_write_page()'s negative value if it were in error, otherwise
 776  *   @ret.
 777  */
 778 int phy_restore_page(struct phy_device *phydev, int oldpage, int ret)
 779 {
 780         int r;
 781
 782         if (oldpage >= 0) {
 783                 r = __phy_write_page(phydev, oldpage);
 784
 785                 /* Propagate the operation return code if the page write
 786                  * was successful.
 787                  */
 788                 if (ret >= 0 && r < 0)
 789                         ret = r;
 790         } else {
 791                 /* Propagate the phy page selection error code */
 792                 ret = oldpage;
 793         }
 794
 795         phy_unlock_mdio_bus(phydev);
 796
 797         return ret;
 798 }
 799 EXPORT_SYMBOL_GPL(phy_restore_page);
 800
 801 /**
 802  * phy_read_paged() - Convenience function for reading a paged register
 803  * @phydev: a pointer to a &struct phy_device
 804  * @page: the page for the phy
 805  * @regnum: register number
 806  *
 807  * Same rules as for phy_read().
 808  */
 809 int phy_read_paged(struct phy_device *phydev, int page, u32 regnum)
 810 {
 811         int ret = 0, oldpage;
 812
 813         oldpage = phy_select_page(phydev, page);
 814         if (oldpage >= 0)
 815                 ret = __phy_read(phydev, regnum);
 816
 817         return phy_restore_page(phydev, oldpage, ret);
 818 }
 819 EXPORT_SYMBOL(phy_read_paged);
 820
 821 /**
 822  * phy_write_paged() - Convenience function for writing a paged register
 823  * @phydev: a pointer to a &struct phy_device
 824  * @page: the page for the phy
 825  * @regnum: register number
 826  * @val: value to write
 827  *
 828  * Same rules as for phy_write().
 829  */
 830 int phy_write_paged(struct phy_device *phydev, int page, u32 regnum, u16 val)
 831 {
 832         int ret = 0, oldpage;
 833
 834         oldpage = phy_select_page(phydev, page);
 835         if (oldpage >= 0)
 836                 ret = __phy_write(phydev, regnum, val);
 837
 838         return phy_restore_page(phydev, oldpage, ret);
 839 }
 840 EXPORT_SYMBOL(phy_write_paged);
 841
 842 /**
 843  * phy_modify_paged_changed() - Function for modifying a paged register
 844  * @phydev: a pointer to a &struct phy_device
 845  * @page: the page for the phy
 846  * @regnum: register number
 847  * @mask: bit mask of bits to clear
 848  * @set: bit mask of bits to set
 849  *
 850  * Returns negative errno, 0 if there was no change, and 1 in case of change
 851  */
 852 int phy_modify_paged_changed(struct phy_device *phydev, int page, u32 regnum,
 853                              u16 mask, u16 set)
 854 {
 855         int ret = 0, oldpage;
 856
 857         oldpage = phy_select_page(phydev, page);
 858         if (oldpage >= 0)
 859                 ret = __phy_modify_changed(phydev, regnum, mask, set);
 860
 861         return phy_restore_page(phydev, oldpage, ret);
 862 }
 863 EXPORT_SYMBOL(phy_modify_paged_changed);
 864
 865 /**
 866  * phy_modify_paged() - Convenience function for modifying a paged register
 867  * @phydev: a pointer to a &struct phy_device
 868  * @page: the page for the phy
 869  * @regnum: register number
 870  * @mask: bit mask of bits to clear
 871  * @set: bit mask of bits to set
 872  *
 873  * Same rules as for phy_read() and phy_write().
 874  */
 875 int phy_modify_paged(struct phy_device *phydev, int page, u32 regnum,
 876                      u16 mask, u16 set)
 877 {
 878         int ret = phy_modify_paged_changed(phydev, page, regnum, mask, set);
 879
 880         return ret < 0 ? ret : 0;
 881 }
 882 EXPORT_SYMBOL(phy_modify_paged);