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