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 != 74,
  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 static int phy_resolve_min_speed(struct phy_device *phydev, bool fdx_only)
 333 {
 334         __ETHTOOL_DECLARE_LINK_MODE_MASK(common);
 335         int i = ARRAY_SIZE(settings);
 336
 337         linkmode_and(common, phydev->lp_advertising, phydev->advertising);
 338
 339         while (--i >= 0) {
 340                 if (test_bit(settings[i].bit, common)) {
 341                         if (fdx_only && settings[i].duplex != DUPLEX_FULL)
 342                                 continue;
 343                         return settings[i].speed;
 344                 }
 345         }
 346
 347         return SPEED_UNKNOWN;
 348 }
 349
 350 int phy_speed_down_core(struct phy_device *phydev)
 351 {
 352         int min_common_speed = phy_resolve_min_speed(phydev, true);
 353
 354         if (min_common_speed == SPEED_UNKNOWN)
 355                 return -EINVAL;
 356
 357         return __set_linkmode_max_speed(min_common_speed, phydev->advertising);
 358 }
 359
 360 static void mmd_phy_indirect(struct mii_bus *bus, int phy_addr, int devad,
 361                              u16 regnum)
 362 {
 363         /* Write the desired MMD Devad */
 364         __mdiobus_write(bus, phy_addr, MII_MMD_CTRL, devad);
 365
 366         /* Write the desired MMD register address */
 367         __mdiobus_write(bus, phy_addr, MII_MMD_DATA, regnum);
 368
 369         /* Select the Function : DATA with no post increment */
 370         __mdiobus_write(bus, phy_addr, MII_MMD_CTRL,
 371                         devad | MII_MMD_CTRL_NOINCR);
 372 }
 373
 374 /**
 375  * __phy_read_mmd - Convenience function for reading a register
 376  * from an MMD on a given PHY.
 377  * @phydev: The phy_device struct
 378  * @devad: The MMD to read from (0..31)
 379  * @regnum: The register on the MMD to read (0..65535)
 380  *
 381  * Same rules as for __phy_read();
 382  */
 383 int __phy_read_mmd(struct phy_device *phydev, int devad, u32 regnum)
 384 {
 385         int val;
 386
 387         if (regnum > (u16)~0 || devad > 32)
 388                 return -EINVAL;
 389
 390         if (phydev->drv->read_mmd) {
 391                 val = phydev->drv->read_mmd(phydev, devad, regnum);
 392         } else if (phydev->is_c45) {
 393                 u32 addr = MII_ADDR_C45 | (devad << 16) | (regnum & 0xffff);
 394
 395                 val = __mdiobus_read(phydev->mdio.bus, phydev->mdio.addr, addr);
 396         } else {
 397                 struct mii_bus *bus = phydev->mdio.bus;
 398                 int phy_addr = phydev->mdio.addr;
 399
 400                 mmd_phy_indirect(bus, phy_addr, devad, regnum);
 401
 402                 /* Read the content of the MMD's selected register */
 403                 val = __mdiobus_read(bus, phy_addr, MII_MMD_DATA);
 404         }
 405         return val;
 406 }
 407 EXPORT_SYMBOL(__phy_read_mmd);
 408
 409 /**
 410  * phy_read_mmd - Convenience function for reading a register
 411  * from an MMD on a given PHY.
 412  * @phydev: The phy_device struct
 413  * @devad: The MMD to read from
 414  * @regnum: The register on the MMD to read
 415  *
 416  * Same rules as for phy_read();
 417  */
 418 int phy_read_mmd(struct phy_device *phydev, int devad, u32 regnum)
 419 {
 420         int ret;
 421
 422         phy_lock_mdio_bus(phydev);
 423         ret = __phy_read_mmd(phydev, devad, regnum);
 424         phy_unlock_mdio_bus(phydev);
 425
 426         return ret;
 427 }
 428 EXPORT_SYMBOL(phy_read_mmd);
 429
 430 /**
 431  * __phy_write_mmd - Convenience function for writing a register
 432  * on an MMD on a given PHY.
 433  * @phydev: The phy_device struct
 434  * @devad: The MMD to read from
 435  * @regnum: The register on the MMD to read
 436  * @val: value to write to @regnum
 437  *
 438  * Same rules as for __phy_write();
 439  */
 440 int __phy_write_mmd(struct phy_device *phydev, int devad, u32 regnum, u16 val)
 441 {
 442         int ret;
 443
 444         if (regnum > (u16)~0 || devad > 32)
 445                 return -EINVAL;
 446
 447         if (phydev->drv->write_mmd) {
 448                 ret = phydev->drv->write_mmd(phydev, devad, regnum, val);
 449         } else if (phydev->is_c45) {
 450                 u32 addr = MII_ADDR_C45 | (devad << 16) | (regnum & 0xffff);
 451
 452                 ret = __mdiobus_write(phydev->mdio.bus, phydev->mdio.addr,
 453                                       addr, val);
 454         } else {
 455                 struct mii_bus *bus = phydev->mdio.bus;
 456                 int phy_addr = phydev->mdio.addr;
 457
 458                 mmd_phy_indirect(bus, phy_addr, devad, regnum);
 459
 460                 /* Write the data into MMD's selected register */
 461                 __mdiobus_write(bus, phy_addr, MII_MMD_DATA, val);
 462
 463                 ret = 0;
 464         }
 465         return ret;
 466 }
 467 EXPORT_SYMBOL(__phy_write_mmd);
 468
 469 /**
 470  * phy_write_mmd - Convenience function for writing a register
 471  * on an MMD on a given PHY.
 472  * @phydev: The phy_device struct
 473  * @devad: The MMD to read from
 474  * @regnum: The register on the MMD to read
 475  * @val: value to write to @regnum
 476  *
 477  * Same rules as for phy_write();
 478  */
 479 int phy_write_mmd(struct phy_device *phydev, int devad, u32 regnum, u16 val)
 480 {
 481         int ret;
 482
 483         phy_lock_mdio_bus(phydev);
 484         ret = __phy_write_mmd(phydev, devad, regnum, val);
 485         phy_unlock_mdio_bus(phydev);
 486
 487         return ret;
 488 }
 489 EXPORT_SYMBOL(phy_write_mmd);
 490
 491 /**
 492  * __phy_modify_changed() - Convenience function for modifying a PHY register
 493  * @phydev: a pointer to a &struct phy_device
 494  * @regnum: register number
 495  * @mask: bit mask of bits to clear
 496  * @set: bit mask of bits to set
 497  *
 498  * Unlocked helper function which allows a PHY register to be modified as
 499  * new register value = (old register value & ~mask) | set
 500  *
 501  * Returns negative errno, 0 if there was no change, and 1 in case of change
 502  */
 503 int __phy_modify_changed(struct phy_device *phydev, u32 regnum, u16 mask,
 504                          u16 set)
 505 {
 506         int new, ret;
 507
 508         ret = __phy_read(phydev, regnum);
 509         if (ret < 0)
 510                 return ret;
 511
 512         new = (ret & ~mask) | set;
 513         if (new == ret)
 514                 return 0;
 515
 516         ret = __phy_write(phydev, regnum, new);
 517
 518         return ret < 0 ? ret : 1;
 519 }
 520 EXPORT_SYMBOL_GPL(__phy_modify_changed);
 521
 522 /**
 523  * phy_modify_changed - Function for modifying a PHY register
 524  * @phydev: the phy_device struct
 525  * @regnum: register number to modify
 526  * @mask: bit mask of bits to clear
 527  * @set: new value of bits set in mask to write to @regnum
 528  *
 529  * NOTE: MUST NOT be called from interrupt context,
 530  * because the bus read/write functions may wait for an interrupt
 531  * to conclude the operation.
 532  *
 533  * Returns negative errno, 0 if there was no change, and 1 in case of change
 534  */
 535 int phy_modify_changed(struct phy_device *phydev, u32 regnum, u16 mask, u16 set)
 536 {
 537         int ret;
 538
 539         phy_lock_mdio_bus(phydev);
 540         ret = __phy_modify_changed(phydev, regnum, mask, set);
 541         phy_unlock_mdio_bus(phydev);
 542
 543         return ret;
 544 }
 545 EXPORT_SYMBOL_GPL(phy_modify_changed);
 546
 547 /**
 548  * __phy_modify - Convenience function for modifying a PHY register
 549  * @phydev: the phy_device struct
 550  * @regnum: register number to modify
 551  * @mask: bit mask of bits to clear
 552  * @set: new value of bits set in mask to write to @regnum
 553  *
 554  * NOTE: MUST NOT be called from interrupt context,
 555  * because the bus read/write functions may wait for an interrupt
 556  * to conclude the operation.
 557  */
 558 int __phy_modify(struct phy_device *phydev, u32 regnum, u16 mask, u16 set)
 559 {
 560         int ret;
 561
 562         ret = __phy_modify_changed(phydev, regnum, mask, set);
 563
 564         return ret < 0 ? ret : 0;
 565 }
 566 EXPORT_SYMBOL_GPL(__phy_modify);
 567
 568 /**
 569  * phy_modify - Convenience function for modifying a given PHY register
 570  * @phydev: the phy_device struct
 571  * @regnum: register number to write
 572  * @mask: bit mask of bits to clear
 573  * @set: new value of bits set in mask to write to @regnum
 574  *
 575  * NOTE: MUST NOT be called from interrupt context,
 576  * because the bus read/write functions may wait for an interrupt
 577  * to conclude the operation.
 578  */
 579 int phy_modify(struct phy_device *phydev, u32 regnum, u16 mask, u16 set)
 580 {
 581         int ret;
 582
 583         phy_lock_mdio_bus(phydev);
 584         ret = __phy_modify(phydev, regnum, mask, set);
 585         phy_unlock_mdio_bus(phydev);
 586
 587         return ret;
 588 }
 589 EXPORT_SYMBOL_GPL(phy_modify);
 590
 591 /**
 592  * __phy_modify_mmd_changed - Function for modifying a register on MMD
 593  * @phydev: the phy_device struct
 594  * @devad: the MMD containing register to modify
 595  * @regnum: register number to modify
 596  * @mask: bit mask of bits to clear
 597  * @set: new value of bits set in mask to write to @regnum
 598  *
 599  * Unlocked helper function which allows a MMD register to be modified as
 600  * new register value = (old register value & ~mask) | set
 601  *
 602  * Returns negative errno, 0 if there was no change, and 1 in case of change
 603  */
 604 int __phy_modify_mmd_changed(struct phy_device *phydev, int devad, u32 regnum,
 605                              u16 mask, u16 set)
 606 {
 607         int new, ret;
 608
 609         ret = __phy_read_mmd(phydev, devad, regnum);
 610         if (ret < 0)
 611                 return ret;
 612
 613         new = (ret & ~mask) | set;
 614         if (new == ret)
 615                 return 0;
 616
 617         ret = __phy_write_mmd(phydev, devad, regnum, new);
 618
 619         return ret < 0 ? ret : 1;
 620 }
 621 EXPORT_SYMBOL_GPL(__phy_modify_mmd_changed);
 622
 623 /**
 624  * phy_modify_mmd_changed - Function for modifying a register on MMD
 625  * @phydev: the phy_device struct
 626  * @devad: the MMD containing register to modify
 627  * @regnum: register number to modify
 628  * @mask: bit mask of bits to clear
 629  * @set: new value of bits set in mask to write to @regnum
 630  *
 631  * NOTE: MUST NOT be called from interrupt context,
 632  * because the bus read/write functions may wait for an interrupt
 633  * to conclude the operation.
 634  *
 635  * Returns negative errno, 0 if there was no change, and 1 in case of change
 636  */
 637 int phy_modify_mmd_changed(struct phy_device *phydev, int devad, u32 regnum,
 638                            u16 mask, u16 set)
 639 {
 640         int ret;
 641
 642         phy_lock_mdio_bus(phydev);
 643         ret = __phy_modify_mmd_changed(phydev, devad, regnum, mask, set);
 644         phy_unlock_mdio_bus(phydev);
 645
 646         return ret;
 647 }
 648 EXPORT_SYMBOL_GPL(phy_modify_mmd_changed);
 649
 650 /**
 651  * __phy_modify_mmd - Convenience function for modifying a register on MMD
 652  * @phydev: the phy_device struct
 653  * @devad: the MMD containing register to modify
 654  * @regnum: register number to modify
 655  * @mask: bit mask of bits to clear
 656  * @set: new value of bits set in mask to write to @regnum
 657  *
 658  * NOTE: MUST NOT be called from interrupt context,
 659  * because the bus read/write functions may wait for an interrupt
 660  * to conclude the operation.
 661  */
 662 int __phy_modify_mmd(struct phy_device *phydev, int devad, u32 regnum,
 663                      u16 mask, u16 set)
 664 {
 665         int ret;
 666
 667         ret = __phy_modify_mmd_changed(phydev, devad, regnum, mask, set);
 668
 669         return ret < 0 ? ret : 0;
 670 }
 671 EXPORT_SYMBOL_GPL(__phy_modify_mmd);
 672
 673 /**
 674  * phy_modify_mmd - Convenience function for modifying a register on MMD
 675  * @phydev: the phy_device struct
 676  * @devad: the MMD containing register to modify
 677  * @regnum: register number to modify
 678  * @mask: bit mask of bits to clear
 679  * @set: new value of bits set in mask to write to @regnum
 680  *
 681  * NOTE: MUST NOT be called from interrupt context,
 682  * because the bus read/write functions may wait for an interrupt
 683  * to conclude the operation.
 684  */
 685 int phy_modify_mmd(struct phy_device *phydev, int devad, u32 regnum,
 686                    u16 mask, u16 set)
 687 {
 688         int ret;
 689
 690         phy_lock_mdio_bus(phydev);
 691         ret = __phy_modify_mmd(phydev, devad, regnum, mask, set);
 692         phy_unlock_mdio_bus(phydev);
 693
 694         return ret;
 695 }
 696 EXPORT_SYMBOL_GPL(phy_modify_mmd);
 697
 698 static int __phy_read_page(struct phy_device *phydev)
 699 {
 700         if (WARN_ONCE(!phydev->drv->read_page, "read_page callback not available, PHY driver not loaded?\n"))
 701                 return -EOPNOTSUPP;
 702
 703         return phydev->drv->read_page(phydev);
 704 }
 705
 706 static int __phy_write_page(struct phy_device *phydev, int page)
 707 {
 708         if (WARN_ONCE(!phydev->drv->write_page, "write_page callback not available, PHY driver not loaded?\n"))
 709                 return -EOPNOTSUPP;
 710
 711         return phydev->drv->write_page(phydev, page);
 712 }
 713
 714 /**
 715  * phy_save_page() - take the bus lock and save the current page
 716  * @phydev: a pointer to a &struct phy_device
 717  *
 718  * Take the MDIO bus lock, and return the current page number. On error,
 719  * returns a negative errno. phy_restore_page() must always be called
 720  * after this, irrespective of success or failure of this call.
 721  */
 722 int phy_save_page(struct phy_device *phydev)
 723 {
 724         phy_lock_mdio_bus(phydev);
 725         return __phy_read_page(phydev);
 726 }
 727 EXPORT_SYMBOL_GPL(phy_save_page);
 728
 729 /**
 730  * phy_select_page() - take the bus lock, save the current page, and set a page
 731  * @phydev: a pointer to a &struct phy_device
 732  * @page: desired page
 733  *
 734  * Take the MDIO bus lock to protect against concurrent access, save the
 735  * current PHY page, and set the current page.  On error, returns a
 736  * negative errno, otherwise returns the previous page number.
 737  * phy_restore_page() must always be called after this, irrespective
 738  * of success or failure of this call.
 739  */
 740 int phy_select_page(struct phy_device *phydev, int page)
 741 {
 742         int ret, oldpage;
 743
 744         oldpage = ret = phy_save_page(phydev);
 745         if (ret < 0)
 746                 return ret;
 747
 748         if (oldpage != page) {
 749                 ret = __phy_write_page(phydev, page);
 750                 if (ret < 0)
 751                         return ret;
 752         }
 753
 754         return oldpage;
 755 }
 756 EXPORT_SYMBOL_GPL(phy_select_page);
 757
 758 /**
 759  * phy_restore_page() - restore the page register and release the bus lock
 760  * @phydev: a pointer to a &struct phy_device
 761  * @oldpage: the old page, return value from phy_save_page() or phy_select_page()
 762  * @ret: operation's return code
 763  *
 764  * Release the MDIO bus lock, restoring @oldpage if it is a valid page.
 765  * This function propagates the earliest error code from the group of
 766  * operations.
 767  *
 768  * Returns:
 769  *   @oldpage if it was a negative value, otherwise
 770  *   @ret if it was a negative errno value, otherwise
 771  *   phy_write_page()'s negative value if it were in error, otherwise
 772  *   @ret.
 773  */
 774 int phy_restore_page(struct phy_device *phydev, int oldpage, int ret)
 775 {
 776         int r;
 777
 778         if (oldpage >= 0) {
 779                 r = __phy_write_page(phydev, oldpage);
 780
 781                 /* Propagate the operation return code if the page write
 782                  * was successful.
 783                  */
 784                 if (ret >= 0 && r < 0)
 785                         ret = r;
 786         } else {
 787                 /* Propagate the phy page selection error code */
 788                 ret = oldpage;
 789         }
 790
 791         phy_unlock_mdio_bus(phydev);
 792
 793         return ret;
 794 }
 795 EXPORT_SYMBOL_GPL(phy_restore_page);
 796
 797 /**
 798  * phy_read_paged() - Convenience function for reading a paged register
 799  * @phydev: a pointer to a &struct phy_device
 800  * @page: the page for the phy
 801  * @regnum: register number
 802  *
 803  * Same rules as for phy_read().
 804  */
 805 int phy_read_paged(struct phy_device *phydev, int page, u32 regnum)
 806 {
 807         int ret = 0, oldpage;
 808
 809         oldpage = phy_select_page(phydev, page);
 810         if (oldpage >= 0)
 811                 ret = __phy_read(phydev, regnum);
 812
 813         return phy_restore_page(phydev, oldpage, ret);
 814 }
 815 EXPORT_SYMBOL(phy_read_paged);
 816
 817 /**
 818  * phy_write_paged() - Convenience function for writing a paged register
 819  * @phydev: a pointer to a &struct phy_device
 820  * @page: the page for the phy
 821  * @regnum: register number
 822  * @val: value to write
 823  *
 824  * Same rules as for phy_write().
 825  */
 826 int phy_write_paged(struct phy_device *phydev, int page, u32 regnum, u16 val)
 827 {
 828         int ret = 0, oldpage;
 829
 830         oldpage = phy_select_page(phydev, page);
 831         if (oldpage >= 0)
 832                 ret = __phy_write(phydev, regnum, val);
 833
 834         return phy_restore_page(phydev, oldpage, ret);
 835 }
 836 EXPORT_SYMBOL(phy_write_paged);
 837
 838 /**
 839  * phy_modify_paged_changed() - Function for modifying a paged register
 840  * @phydev: a pointer to a &struct phy_device
 841  * @page: the page for the phy
 842  * @regnum: register number
 843  * @mask: bit mask of bits to clear
 844  * @set: bit mask of bits to set
 845  *
 846  * Returns negative errno, 0 if there was no change, and 1 in case of change
 847  */
 848 int phy_modify_paged_changed(struct phy_device *phydev, int page, u32 regnum,
 849                              u16 mask, u16 set)
 850 {
 851         int ret = 0, oldpage;
 852
 853         oldpage = phy_select_page(phydev, page);
 854         if (oldpage >= 0)
 855                 ret = __phy_modify_changed(phydev, regnum, mask, set);
 856
 857         return phy_restore_page(phydev, oldpage, ret);
 858 }
 859 EXPORT_SYMBOL(phy_modify_paged_changed);
 860
 861 /**
 862  * phy_modify_paged() - Convenience function for modifying a paged register
 863  * @phydev: a pointer to a &struct phy_device
 864  * @page: the page for the phy
 865  * @regnum: register number
 866  * @mask: bit mask of bits to clear
 867  * @set: bit mask of bits to set
 868  *
 869  * Same rules as for phy_read() and phy_write().
 870  */
 871 int phy_modify_paged(struct phy_device *phydev, int page, u32 regnum,
 872                      u16 mask, u16 set)
 873 {
 874         int ret = phy_modify_paged_changed(phydev, page, regnum, mask, set);
 875
 876         return ret < 0 ? ret : 0;
 877 }
 878 EXPORT_SYMBOL(phy_modify_paged);