drivers/net/phy/phy_device.c

   1 /*
   2  * drivers/net/phy/phy_device.c
   3  *
   4  * Framework for finding and configuring PHYs.
   5  * Also contains generic PHY driver
   6  *
   7  * Author: Andy Fleming
   8  *
   9  * Copyright (c) 2004 Freescale Semiconductor, Inc.
  10  *
  11  * This program is free software; you can redistribute  it and/or modify it
  12  * under  the terms of  the GNU General  Public License as published by the
  13  * Free Software Foundation;  either version 2 of the  License, or (at your
  14  * option) any later version.
  15  *
  16  */
  17
  18 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
  19
  20 #include <linux/kernel.h>
  21 #include <linux/string.h>
  22 #include <linux/errno.h>
  23 #include <linux/unistd.h>
  24 #include <linux/slab.h>
  25 #include <linux/interrupt.h>
  26 #include <linux/init.h>
  27 #include <linux/delay.h>
  28 #include <linux/netdevice.h>
  29 #include <linux/etherdevice.h>
  30 #include <linux/skbuff.h>
  31 #include <linux/mm.h>
  32 #include <linux/module.h>
  33 #include <linux/mii.h>
  34 #include <linux/ethtool.h>
  35 #include <linux/phy.h>
  36
  37 #include <asm/io.h>
  38 #include <asm/irq.h>
  39 #include <asm/uaccess.h>
  40
  41 MODULE_DESCRIPTION("PHY library");
  42 MODULE_AUTHOR("Andy Fleming");
  43 MODULE_LICENSE("GPL");
  44
  45 void phy_device_free(struct phy_device *phydev)
  46 {
  47         put_device(&phydev->dev);
  48 }
  49 EXPORT_SYMBOL(phy_device_free);
  50
  51 static void phy_device_release(struct device *dev)
  52 {
  53         kfree(to_phy_device(dev));
  54 }
  55
  56 static struct phy_driver genphy_driver;
  57 extern int mdio_bus_init(void);
  58 extern void mdio_bus_exit(void);
  59
  60 static LIST_HEAD(phy_fixup_list);
  61 static DEFINE_MUTEX(phy_fixup_lock);
  62
  63 static int phy_attach_direct(struct net_device *dev, struct phy_device *phydev,
  64                              u32 flags, phy_interface_t interface);
  65
  66 /*
  67  * Creates a new phy_fixup and adds it to the list
  68  * @bus_id: A string which matches phydev->dev.bus_id (or PHY_ANY_ID)
  69  * @phy_uid: Used to match against phydev->phy_id (the UID of the PHY)
  70  *      It can also be PHY_ANY_UID
  71  * @phy_uid_mask: Applied to phydev->phy_id and fixup->phy_uid before
  72  *      comparison
  73  * @run: The actual code to be run when a matching PHY is found
  74  */
  75 int phy_register_fixup(const char *bus_id, u32 phy_uid, u32 phy_uid_mask,
  76                 int (*run)(struct phy_device *))
  77 {
  78         struct phy_fixup *fixup;
  79
  80         fixup = kzalloc(sizeof(struct phy_fixup), GFP_KERNEL);
  81         if (!fixup)
  82                 return -ENOMEM;
  83
  84         strlcpy(fixup->bus_id, bus_id, sizeof(fixup->bus_id));
  85         fixup->phy_uid = phy_uid;
  86         fixup->phy_uid_mask = phy_uid_mask;
  87         fixup->run = run;
  88
  89         mutex_lock(&phy_fixup_lock);
  90         list_add_tail(&fixup->list, &phy_fixup_list);
  91         mutex_unlock(&phy_fixup_lock);
  92
  93         return 0;
  94 }
  95 EXPORT_SYMBOL(phy_register_fixup);
  96
  97 /* Registers a fixup to be run on any PHY with the UID in phy_uid */
  98 int phy_register_fixup_for_uid(u32 phy_uid, u32 phy_uid_mask,
  99                 int (*run)(struct phy_device *))
 100 {
 101         return phy_register_fixup(PHY_ANY_ID, phy_uid, phy_uid_mask, run);
 102 }
 103 EXPORT_SYMBOL(phy_register_fixup_for_uid);
 104
 105 /* Registers a fixup to be run on the PHY with id string bus_id */
 106 int phy_register_fixup_for_id(const char *bus_id,
 107                 int (*run)(struct phy_device *))
 108 {
 109         return phy_register_fixup(bus_id, PHY_ANY_UID, 0xffffffff, run);
 110 }
 111 EXPORT_SYMBOL(phy_register_fixup_for_id);
 112
 113 /*
 114  * Returns 1 if fixup matches phydev in bus_id and phy_uid.
 115  * Fixups can be set to match any in one or more fields.
 116  */
 117 static int phy_needs_fixup(struct phy_device *phydev, struct phy_fixup *fixup)
 118 {
 119         if (strcmp(fixup->bus_id, dev_name(&phydev->dev)) != 0)
 120                 if (strcmp(fixup->bus_id, PHY_ANY_ID) != 0)
 121                         return 0;
 122
 123         if ((fixup->phy_uid & fixup->phy_uid_mask) !=
 124                         (phydev->phy_id & fixup->phy_uid_mask))
 125                 if (fixup->phy_uid != PHY_ANY_UID)
 126                         return 0;
 127
 128         return 1;
 129 }
 130
 131 /* Runs any matching fixups for this phydev */
 132 int phy_scan_fixups(struct phy_device *phydev)
 133 {
 134         struct phy_fixup *fixup;
 135
 136         mutex_lock(&phy_fixup_lock);
 137         list_for_each_entry(fixup, &phy_fixup_list, list) {
 138                 if (phy_needs_fixup(phydev, fixup)) {
 139                         int err;
 140
 141                         err = fixup->run(phydev);
 142
 143                         if (err < 0) {
 144                                 mutex_unlock(&phy_fixup_lock);
 145                                 return err;
 146                         }
 147                 }
 148         }
 149         mutex_unlock(&phy_fixup_lock);
 150
 151         return 0;
 152 }
 153 EXPORT_SYMBOL(phy_scan_fixups);
 154
 155 struct phy_device *phy_device_create(struct mii_bus *bus, int addr, int phy_id,
 156                         bool is_c45, struct phy_c45_device_ids *c45_ids)
 157 {
 158         struct phy_device *dev;
 159
 160         /* We allocate the device, and initialize the
 161          * default values */
 162         dev = kzalloc(sizeof(*dev), GFP_KERNEL);
 163
 164         if (NULL == dev)
 165                 return (struct phy_device*) PTR_ERR((void*)-ENOMEM);
 166
 167         dev->dev.release = phy_device_release;
 168
 169         dev->speed = 0;
 170         dev->duplex = -1;
 171         dev->pause = dev->asym_pause = 0;
 172         dev->link = 1;
 173         dev->interface = PHY_INTERFACE_MODE_GMII;
 174
 175         dev->autoneg = AUTONEG_ENABLE;
 176
 177         dev->is_c45 = is_c45;
 178         dev->addr = addr;
 179         dev->phy_id = phy_id;
 180         if (c45_ids)
 181                 dev->c45_ids = *c45_ids;
 182         dev->bus = bus;
 183         dev->dev.parent = bus->parent;
 184         dev->dev.bus = &mdio_bus_type;
 185         dev->irq = bus->irq != NULL ? bus->irq[addr] : PHY_POLL;
 186         dev_set_name(&dev->dev, PHY_ID_FMT, bus->id, addr);
 187
 188         dev->state = PHY_DOWN;
 189
 190         mutex_init(&dev->lock);
 191         INIT_DELAYED_WORK(&dev->state_queue, phy_state_machine);
 192
 193         /* Request the appropriate module unconditionally; don't
 194            bother trying to do so only if it isn't already loaded,
 195            because that gets complicated. A hotplug event would have
 196            done an unconditional modprobe anyway.
 197            We don't do normal hotplug because it won't work for MDIO
 198            -- because it relies on the device staying around for long
 199            enough for the driver to get loaded. With MDIO, the NIC
 200            driver will get bored and give up as soon as it finds that
 201            there's no driver _already_ loaded. */
 202         request_module(MDIO_MODULE_PREFIX MDIO_ID_FMT, MDIO_ID_ARGS(phy_id));
 203
 204         device_initialize(&dev->dev);
 205
 206         return dev;
 207 }
 208 EXPORT_SYMBOL(phy_device_create);
 209
 210 /**
 211  * get_phy_c45_ids - reads the specified addr for its 802.3-c45 IDs.
 212  * @bus: the target MII bus
 213  * @addr: PHY address on the MII bus
 214  * @phy_id: where to store the ID retrieved.
 215  * @c45_ids: where to store the c45 ID information.
 216  *
 217  *   If the PHY devices-in-package appears to be valid, it and the
 218  *   corresponding identifiers are stored in @c45_ids, zero is stored
 219  *   in @phy_id.  Otherwise 0xffffffff is stored in @phy_id.  Returns
 220  *   zero on success.
 221  *
 222  */
 223 static int get_phy_c45_ids(struct mii_bus *bus, int addr, u32 *phy_id,
 224                            struct phy_c45_device_ids *c45_ids) {
 225         int phy_reg;
 226         int i, reg_addr;
 227         const int num_ids = ARRAY_SIZE(c45_ids->device_ids);
 228
 229         /* Find first non-zero Devices In package.  Device
 230          * zero is reserved, so don't probe it.
 231          */
 232         for (i = 1;
 233              i < num_ids && c45_ids->devices_in_package == 0;
 234              i++) {
 235                 reg_addr = MII_ADDR_C45 | i << 16 | 6;
 236                 phy_reg = mdiobus_read(bus, addr, reg_addr);
 237                 if (phy_reg < 0)
 238                         return -EIO;
 239                 c45_ids->devices_in_package = (phy_reg & 0xffff) << 16;
 240
 241                 reg_addr = MII_ADDR_C45 | i << 16 | 5;
 242                 phy_reg = mdiobus_read(bus, addr, reg_addr);
 243                 if (phy_reg < 0)
 244                         return -EIO;
 245                 c45_ids->devices_in_package |= (phy_reg & 0xffff);
 246
 247                 /* If mostly Fs, there is no device there,
 248                  * let's get out of here.
 249                  */
 250                 if ((c45_ids->devices_in_package & 0x1fffffff) == 0x1fffffff) {
 251                         *phy_id = 0xffffffff;
 252                         return 0;
 253                 }
 254         }
 255
 256         /* Now probe Device Identifiers for each device present. */
 257         for (i = 1; i < num_ids; i++) {
 258                 if (!(c45_ids->devices_in_package & (1 << i)))
 259                         continue;
 260
 261                 reg_addr = MII_ADDR_C45 | i << 16 | MII_PHYSID1;
 262                 phy_reg = mdiobus_read(bus, addr, reg_addr);
 263                 if (phy_reg < 0)
 264                         return -EIO;
 265                 c45_ids->device_ids[i] = (phy_reg & 0xffff) << 16;
 266
 267                 reg_addr = MII_ADDR_C45 | i << 16 | MII_PHYSID2;
 268                 phy_reg = mdiobus_read(bus, addr, reg_addr);
 269                 if (phy_reg < 0)
 270                         return -EIO;
 271                 c45_ids->device_ids[i] |= (phy_reg & 0xffff);
 272         }
 273         *phy_id = 0;
 274         return 0;
 275 }
 276
 277 /**
 278  * get_phy_id - reads the specified addr for its ID.
 279  * @bus: the target MII bus
 280  * @addr: PHY address on the MII bus
 281  * @phy_id: where to store the ID retrieved.
 282  * @is_c45: If true the PHY uses the 802.3 clause 45 protocol
 283  * @c45_ids: where to store the c45 ID information.
 284  *
 285  * Description: In the case of a 802.3-c22 PHY, reads the ID registers
 286  *   of the PHY at @addr on the @bus, stores it in @phy_id and returns
 287  *   zero on success.
 288  *
 289  *   In the case of a 802.3-c45 PHY, get_phy_c45_ids() is invoked, and
 290  *   its return value is in turn returned.
 291  *
 292  */
 293 static int get_phy_id(struct mii_bus *bus, int addr, u32 *phy_id,
 294                       bool is_c45, struct phy_c45_device_ids *c45_ids)
 295 {
 296         int phy_reg;
 297
 298         if (is_c45)
 299                 return get_phy_c45_ids(bus, addr, phy_id, c45_ids);
 300
 301         /* Grab the bits from PHYIR1, and put them
 302          * in the upper half */
 303         phy_reg = mdiobus_read(bus, addr, MII_PHYSID1);
 304
 305         if (phy_reg < 0)
 306                 return -EIO;
 307
 308         *phy_id = (phy_reg & 0xffff) << 16;
 309
 310         /* Grab the bits from PHYIR2, and put them in the lower half */
 311         phy_reg = mdiobus_read(bus, addr, MII_PHYSID2);
 312
 313         if (phy_reg < 0)
 314                 return -EIO;
 315
 316         *phy_id |= (phy_reg & 0xffff);
 317
 318         return 0;
 319 }
 320
 321 /**
 322  * get_phy_device - reads the specified PHY device and returns its @phy_device struct
 323  * @bus: the target MII bus
 324  * @addr: PHY address on the MII bus
 325  * @is_c45: If true the PHY uses the 802.3 clause 45 protocol
 326  *
 327  * Description: Reads the ID registers of the PHY at @addr on the
 328  *   @bus, then allocates and returns the phy_device to represent it.
 329  */
 330 struct phy_device *get_phy_device(struct mii_bus *bus, int addr, bool is_c45)
 331 {
 332         struct phy_c45_device_ids c45_ids = {0};
 333         struct phy_device *dev = NULL;
 334         u32 phy_id = 0;
 335         int r;
 336
 337         r = get_phy_id(bus, addr, &phy_id, is_c45, &c45_ids);
 338         if (r)
 339                 return ERR_PTR(r);
 340
 341         /* If the phy_id is mostly Fs, there is no device there */
 342         if ((phy_id & 0x1fffffff) == 0x1fffffff)
 343                 return NULL;
 344
 345         dev = phy_device_create(bus, addr, phy_id, is_c45, &c45_ids);
 346
 347         return dev;
 348 }
 349 EXPORT_SYMBOL(get_phy_device);
 350
 351 /**
 352  * phy_device_register - Register the phy device on the MDIO bus
 353  * @phydev: phy_device structure to be added to the MDIO bus
 354  */
 355 int phy_device_register(struct phy_device *phydev)
 356 {
 357         int err;
 358
 359         /* Don't register a phy if one is already registered at this
 360          * address */
 361         if (phydev->bus->phy_map[phydev->addr])
 362                 return -EINVAL;
 363         phydev->bus->phy_map[phydev->addr] = phydev;
 364
 365         /* Run all of the fixups for this PHY */
 366         phy_scan_fixups(phydev);
 367
 368         err = device_add(&phydev->dev);
 369         if (err) {
 370                 pr_err("PHY %d failed to add\n", phydev->addr);
 371                 goto out;
 372         }
 373
 374         return 0;
 375
 376  out:
 377         phydev->bus->phy_map[phydev->addr] = NULL;
 378         return err;
 379 }
 380 EXPORT_SYMBOL(phy_device_register);
 381
 382 /**
 383  * phy_find_first - finds the first PHY device on the bus
 384  * @bus: the target MII bus
 385  */
 386 struct phy_device *phy_find_first(struct mii_bus *bus)
 387 {
 388         int addr;
 389
 390         for (addr = 0; addr < PHY_MAX_ADDR; addr++) {
 391                 if (bus->phy_map[addr])
 392                         return bus->phy_map[addr];
 393         }
 394         return NULL;
 395 }
 396 EXPORT_SYMBOL(phy_find_first);
 397
 398 /**
 399  * phy_prepare_link - prepares the PHY layer to monitor link status
 400  * @phydev: target phy_device struct
 401  * @handler: callback function for link status change notifications
 402  *
 403  * Description: Tells the PHY infrastructure to handle the
 404  *   gory details on monitoring link status (whether through
 405  *   polling or an interrupt), and to call back to the
 406  *   connected device driver when the link status changes.
 407  *   If you want to monitor your own link state, don't call
 408  *   this function.
 409  */
 410 static void phy_prepare_link(struct phy_device *phydev,
 411                 void (*handler)(struct net_device *))
 412 {
 413         phydev->adjust_link = handler;
 414 }
 415
 416 /**
 417  * phy_connect_direct - connect an ethernet device to a specific phy_device
 418  * @dev: the network device to connect
 419  * @phydev: the pointer to the phy device
 420  * @handler: callback function for state change notifications
 421  * @interface: PHY device's interface
 422  */
 423 int phy_connect_direct(struct net_device *dev, struct phy_device *phydev,
 424                        void (*handler)(struct net_device *),
 425                        phy_interface_t interface)
 426 {
 427         int rc;
 428
 429         rc = phy_attach_direct(dev, phydev, phydev->dev_flags, interface);
 430         if (rc)
 431                 return rc;
 432
 433         phy_prepare_link(phydev, handler);
 434         phy_start_machine(phydev, NULL);
 435         if (phydev->irq > 0)
 436                 phy_start_interrupts(phydev);
 437
 438         return 0;
 439 }
 440 EXPORT_SYMBOL(phy_connect_direct);
 441
 442 /**
 443  * phy_connect - connect an ethernet device to a PHY device
 444  * @dev: the network device to connect
 445  * @bus_id: the id string of the PHY device to connect
 446  * @handler: callback function for state change notifications
 447  * @interface: PHY device's interface
 448  *
 449  * Description: Convenience function for connecting ethernet
 450  *   devices to PHY devices.  The default behavior is for
 451  *   the PHY infrastructure to handle everything, and only notify
 452  *   the connected driver when the link status changes.  If you
 453  *   don't want, or can't use the provided functionality, you may
 454  *   choose to call only the subset of functions which provide
 455  *   the desired functionality.
 456  */
 457 struct phy_device * phy_connect(struct net_device *dev, const char *bus_id,
 458                 void (*handler)(struct net_device *),
 459                 phy_interface_t interface)
 460 {
 461         struct phy_device *phydev;
 462         struct device *d;
 463         int rc;
 464
 465         /* Search the list of PHY devices on the mdio bus for the
 466          * PHY with the requested name */
 467         d = bus_find_device_by_name(&mdio_bus_type, NULL, bus_id);
 468         if (!d) {
 469                 pr_err("PHY %s not found\n", bus_id);
 470                 return ERR_PTR(-ENODEV);
 471         }
 472         phydev = to_phy_device(d);
 473
 474         rc = phy_connect_direct(dev, phydev, handler, interface);
 475         if (rc)
 476                 return ERR_PTR(rc);
 477
 478         return phydev;
 479 }
 480 EXPORT_SYMBOL(phy_connect);
 481
 482 /**
 483  * phy_disconnect - disable interrupts, stop state machine, and detach a PHY device
 484  * @phydev: target phy_device struct
 485  */
 486 void phy_disconnect(struct phy_device *phydev)
 487 {
 488         if (phydev->irq > 0)
 489                 phy_stop_interrupts(phydev);
 490
 491         phy_stop_machine(phydev);
 492
 493         phydev->adjust_link = NULL;
 494
 495         phy_detach(phydev);
 496 }
 497 EXPORT_SYMBOL(phy_disconnect);
 498
 499 int phy_init_hw(struct phy_device *phydev)
 500 {
 501         int ret;
 502
 503         if (!phydev->drv || !phydev->drv->config_init)
 504                 return 0;
 505
 506         ret = phy_scan_fixups(phydev);
 507         if (ret < 0)
 508                 return ret;
 509
 510         return phydev->drv->config_init(phydev);
 511 }
 512
 513 /**
 514  * phy_attach_direct - attach a network device to a given PHY device pointer
 515  * @dev: network device to attach
 516  * @phydev: Pointer to phy_device to attach
 517  * @flags: PHY device's dev_flags
 518  * @interface: PHY device's interface
 519  *
 520  * Description: Called by drivers to attach to a particular PHY
 521  *     device. The phy_device is found, and properly hooked up
 522  *     to the phy_driver.  If no driver is attached, then the
 523  *     genphy_driver is used.  The phy_device is given a ptr to
 524  *     the attaching device, and given a callback for link status
 525  *     change.  The phy_device is returned to the attaching driver.
 526  */
 527 static int phy_attach_direct(struct net_device *dev, struct phy_device *phydev,
 528                              u32 flags, phy_interface_t interface)
 529 {
 530         struct device *d = &phydev->dev;
 531         int err;
 532
 533         /* Assume that if there is no driver, that it doesn't
 534          * exist, and we should use the genphy driver. */
 535         if (NULL == d->driver) {
 536                 if (phydev->is_c45) {
 537                         pr_err("No driver for phy %x\n", phydev->phy_id);
 538                         return -ENODEV;
 539                 }
 540
 541                 d->driver = &genphy_driver.driver;
 542
 543                 err = d->driver->probe(d);
 544                 if (err >= 0)
 545                         err = device_bind_driver(d);
 546
 547                 if (err)
 548                         return err;
 549         }
 550
 551         if (phydev->attached_dev) {
 552                 dev_err(&dev->dev, "PHY already attached\n");
 553                 return -EBUSY;
 554         }
 555
 556         phydev->attached_dev = dev;
 557         dev->phydev = phydev;
 558
 559         phydev->dev_flags = flags;
 560
 561         phydev->interface = interface;
 562
 563         phydev->state = PHY_READY;
 564
 565         /* Do initial configuration here, now that
 566          * we have certain key parameters
 567          * (dev_flags and interface) */
 568         err = phy_init_hw(phydev);
 569         if (err)
 570                 phy_detach(phydev);
 571
 572         return err;
 573 }
 574
 575 /**
 576  * phy_attach - attach a network device to a particular PHY device
 577  * @dev: network device to attach
 578  * @bus_id: Bus ID of PHY device to attach
 579  * @interface: PHY device's interface
 580  *
 581  * Description: Same as phy_attach_direct() except that a PHY bus_id
 582  *     string is passed instead of a pointer to a struct phy_device.
 583  */
 584 struct phy_device *phy_attach(struct net_device *dev,
 585                 const char *bus_id, phy_interface_t interface)
 586 {
 587         struct bus_type *bus = &mdio_bus_type;
 588         struct phy_device *phydev;
 589         struct device *d;
 590         int rc;
 591
 592         /* Search the list of PHY devices on the mdio bus for the
 593          * PHY with the requested name */
 594         d = bus_find_device_by_name(bus, NULL, bus_id);
 595         if (!d) {
 596                 pr_err("PHY %s not found\n", bus_id);
 597                 return ERR_PTR(-ENODEV);
 598         }
 599         phydev = to_phy_device(d);
 600
 601         rc = phy_attach_direct(dev, phydev, phydev->dev_flags, interface);
 602         if (rc)
 603                 return ERR_PTR(rc);
 604
 605         return phydev;
 606 }
 607 EXPORT_SYMBOL(phy_attach);
 608
 609 /**
 610  * phy_detach - detach a PHY device from its network device
 611  * @phydev: target phy_device struct
 612  */
 613 void phy_detach(struct phy_device *phydev)
 614 {
 615         phydev->attached_dev->phydev = NULL;
 616         phydev->attached_dev = NULL;
 617
 618         /* If the device had no specific driver before (i.e. - it
 619          * was using the generic driver), we unbind the device
 620          * from the generic driver so that there's a chance a
 621          * real driver could be loaded */
 622         if (phydev->dev.driver == &genphy_driver.driver)
 623                 device_release_driver(&phydev->dev);
 624 }
 625 EXPORT_SYMBOL(phy_detach);
 626
 627
 628 /* Generic PHY support and helper functions */
 629
 630 /**
 631  * genphy_config_advert - sanitize and advertise auto-negotiation parameters
 632  * @phydev: target phy_device struct
 633  *
 634  * Description: Writes MII_ADVERTISE with the appropriate values,
 635  *   after sanitizing the values to make sure we only advertise
 636  *   what is supported.  Returns < 0 on error, 0 if the PHY's advertisement
 637  *   hasn't changed, and > 0 if it has changed.
 638  */
 639 static int genphy_config_advert(struct phy_device *phydev)
 640 {
 641         u32 advertise;
 642         int oldadv, adv;
 643         int err, changed = 0;
 644
 645         /* Only allow advertising what
 646          * this PHY supports */
 647         phydev->advertising &= phydev->supported;
 648         advertise = phydev->advertising;
 649
 650         /* Setup standard advertisement */
 651         oldadv = adv = phy_read(phydev, MII_ADVERTISE);
 652
 653         if (adv < 0)
 654                 return adv;
 655
 656         adv &= ~(ADVERTISE_ALL | ADVERTISE_100BASE4 | ADVERTISE_PAUSE_CAP |
 657                  ADVERTISE_PAUSE_ASYM);
 658         adv |= ethtool_adv_to_mii_adv_t(advertise);
 659
 660         if (adv != oldadv) {
 661                 err = phy_write(phydev, MII_ADVERTISE, adv);
 662
 663                 if (err < 0)
 664                         return err;
 665                 changed = 1;
 666         }
 667
 668         /* Configure gigabit if it's supported */
 669         if (phydev->supported & (SUPPORTED_1000baseT_Half |
 670                                 SUPPORTED_1000baseT_Full)) {
 671                 oldadv = adv = phy_read(phydev, MII_CTRL1000);
 672
 673                 if (adv < 0)
 674                         return adv;
 675
 676                 adv &= ~(ADVERTISE_1000FULL | ADVERTISE_1000HALF);
 677                 adv |= ethtool_adv_to_mii_ctrl1000_t(advertise);
 678
 679                 if (adv != oldadv) {
 680                         err = phy_write(phydev, MII_CTRL1000, adv);
 681
 682                         if (err < 0)
 683                                 return err;
 684                         changed = 1;
 685                 }
 686         }
 687
 688         return changed;
 689 }
 690
 691 /**
 692  * genphy_setup_forced - configures/forces speed/duplex from @phydev
 693  * @phydev: target phy_device struct
 694  *
 695  * Description: Configures MII_BMCR to force speed/duplex
 696  *   to the values in phydev. Assumes that the values are valid.
 697  *   Please see phy_sanitize_settings().
 698  */
 699 static int genphy_setup_forced(struct phy_device *phydev)
 700 {
 701         int err;
 702         int ctl = 0;
 703
 704         phydev->pause = phydev->asym_pause = 0;
 705
 706         if (SPEED_1000 == phydev->speed)
 707                 ctl |= BMCR_SPEED1000;
 708         else if (SPEED_100 == phydev->speed)
 709                 ctl |= BMCR_SPEED100;
 710
 711         if (DUPLEX_FULL == phydev->duplex)
 712                 ctl |= BMCR_FULLDPLX;
 713
 714         err = phy_write(phydev, MII_BMCR, ctl);
 715
 716         return err;
 717 }
 718
 719
 720 /**
 721  * genphy_restart_aneg - Enable and Restart Autonegotiation
 722  * @phydev: target phy_device struct
 723  */
 724 int genphy_restart_aneg(struct phy_device *phydev)
 725 {
 726         int ctl;
 727
 728         ctl = phy_read(phydev, MII_BMCR);
 729
 730         if (ctl < 0)
 731                 return ctl;
 732
 733         ctl |= (BMCR_ANENABLE | BMCR_ANRESTART);
 734
 735         /* Don't isolate the PHY if we're negotiating */
 736         ctl &= ~(BMCR_ISOLATE);
 737
 738         ctl = phy_write(phydev, MII_BMCR, ctl);
 739
 740         return ctl;
 741 }
 742 EXPORT_SYMBOL(genphy_restart_aneg);
 743
 744
 745 /**
 746  * genphy_config_aneg - restart auto-negotiation or write BMCR
 747  * @phydev: target phy_device struct
 748  *
 749  * Description: If auto-negotiation is enabled, we configure the
 750  *   advertising, and then restart auto-negotiation.  If it is not
 751  *   enabled, then we write the BMCR.
 752  */
 753 int genphy_config_aneg(struct phy_device *phydev)
 754 {
 755         int result;
 756
 757         if (AUTONEG_ENABLE != phydev->autoneg)
 758                 return genphy_setup_forced(phydev);
 759
 760         result = genphy_config_advert(phydev);
 761
 762         if (result < 0) /* error */
 763                 return result;
 764
 765         if (result == 0) {
 766                 /* Advertisement hasn't changed, but maybe aneg was never on to
 767                  * begin with?  Or maybe phy was isolated? */
 768                 int ctl = phy_read(phydev, MII_BMCR);
 769
 770                 if (ctl < 0)
 771                         return ctl;
 772
 773                 if (!(ctl & BMCR_ANENABLE) || (ctl & BMCR_ISOLATE))
 774                         result = 1; /* do restart aneg */
 775         }
 776
 777         /* Only restart aneg if we are advertising something different
 778          * than we were before.  */
 779         if (result > 0)
 780                 result = genphy_restart_aneg(phydev);
 781
 782         return result;
 783 }
 784 EXPORT_SYMBOL(genphy_config_aneg);
 785
 786 /**
 787  * genphy_update_link - update link status in @phydev
 788  * @phydev: target phy_device struct
 789  *
 790  * Description: Update the value in phydev->link to reflect the
 791  *   current link value.  In order to do this, we need to read
 792  *   the status register twice, keeping the second value.
 793  */
 794 int genphy_update_link(struct phy_device *phydev)
 795 {
 796         int status;
 797
 798         /* Do a fake read */
 799         status = phy_read(phydev, MII_BMSR);
 800
 801         if (status < 0)
 802                 return status;
 803
 804         /* Read link and autonegotiation status */
 805         status = phy_read(phydev, MII_BMSR);
 806
 807         if (status < 0)
 808                 return status;
 809
 810         if ((status & BMSR_LSTATUS) == 0)
 811                 phydev->link = 0;
 812         else
 813                 phydev->link = 1;
 814
 815         return 0;
 816 }
 817 EXPORT_SYMBOL(genphy_update_link);
 818
 819 /**
 820  * genphy_read_status - check the link status and update current link state
 821  * @phydev: target phy_device struct
 822  *
 823  * Description: Check the link, then figure out the current state
 824  *   by comparing what we advertise with what the link partner
 825  *   advertises.  Start by checking the gigabit possibilities,
 826  *   then move on to 10/100.
 827  */
 828 int genphy_read_status(struct phy_device *phydev)
 829 {
 830         int adv;
 831         int err;
 832         int lpa;
 833         int lpagb = 0;
 834
 835         /* Update the link, but return if there
 836          * was an error */
 837         err = genphy_update_link(phydev);
 838         if (err)
 839                 return err;
 840
 841         if (AUTONEG_ENABLE == phydev->autoneg) {
 842                 if (phydev->supported & (SUPPORTED_1000baseT_Half
 843                                         | SUPPORTED_1000baseT_Full)) {
 844                         lpagb = phy_read(phydev, MII_STAT1000);
 845
 846                         if (lpagb < 0)
 847                                 return lpagb;
 848
 849                         adv = phy_read(phydev, MII_CTRL1000);
 850
 851                         if (adv < 0)
 852                                 return adv;
 853
 854                         lpagb &= adv << 2;
 855                 }
 856
 857                 lpa = phy_read(phydev, MII_LPA);
 858
 859                 if (lpa < 0)
 860                         return lpa;
 861
 862                 adv = phy_read(phydev, MII_ADVERTISE);
 863
 864                 if (adv < 0)
 865                         return adv;
 866
 867                 lpa &= adv;
 868
 869                 phydev->speed = SPEED_10;
 870                 phydev->duplex = DUPLEX_HALF;
 871                 phydev->pause = phydev->asym_pause = 0;
 872
 873                 if (lpagb & (LPA_1000FULL | LPA_1000HALF)) {
 874                         phydev->speed = SPEED_1000;
 875
 876                         if (lpagb & LPA_1000FULL)
 877                                 phydev->duplex = DUPLEX_FULL;
 878                 } else if (lpa & (LPA_100FULL | LPA_100HALF)) {
 879                         phydev->speed = SPEED_100;
 880
 881                         if (lpa & LPA_100FULL)
 882                                 phydev->duplex = DUPLEX_FULL;
 883                 } else
 884                         if (lpa & LPA_10FULL)
 885                                 phydev->duplex = DUPLEX_FULL;
 886
 887                 if (phydev->duplex == DUPLEX_FULL){
 888                         phydev->pause = lpa & LPA_PAUSE_CAP ? 1 : 0;
 889                         phydev->asym_pause = lpa & LPA_PAUSE_ASYM ? 1 : 0;
 890                 }
 891         } else {
 892                 int bmcr = phy_read(phydev, MII_BMCR);
 893                 if (bmcr < 0)
 894                         return bmcr;
 895
 896                 if (bmcr & BMCR_FULLDPLX)
 897                         phydev->duplex = DUPLEX_FULL;
 898                 else
 899                         phydev->duplex = DUPLEX_HALF;
 900
 901                 if (bmcr & BMCR_SPEED1000)
 902                         phydev->speed = SPEED_1000;
 903                 else if (bmcr & BMCR_SPEED100)
 904                         phydev->speed = SPEED_100;
 905                 else
 906                         phydev->speed = SPEED_10;
 907
 908                 phydev->pause = phydev->asym_pause = 0;
 909         }
 910
 911         return 0;
 912 }
 913 EXPORT_SYMBOL(genphy_read_status);
 914
 915 static int genphy_config_init(struct phy_device *phydev)
 916 {
 917         int val;
 918         u32 features;
 919
 920         /* For now, I'll claim that the generic driver supports
 921          * all possible port types */
 922         features = (SUPPORTED_TP | SUPPORTED_MII
 923                         | SUPPORTED_AUI | SUPPORTED_FIBRE |
 924                         SUPPORTED_BNC);
 925
 926         /* Do we support autonegotiation? */
 927         val = phy_read(phydev, MII_BMSR);
 928
 929         if (val < 0)
 930                 return val;
 931
 932         if (val & BMSR_ANEGCAPABLE)
 933                 features |= SUPPORTED_Autoneg;
 934
 935         if (val & BMSR_100FULL)
 936                 features |= SUPPORTED_100baseT_Full;
 937         if (val & BMSR_100HALF)
 938                 features |= SUPPORTED_100baseT_Half;
 939         if (val & BMSR_10FULL)
 940                 features |= SUPPORTED_10baseT_Full;
 941         if (val & BMSR_10HALF)
 942                 features |= SUPPORTED_10baseT_Half;
 943
 944         if (val & BMSR_ESTATEN) {
 945                 val = phy_read(phydev, MII_ESTATUS);
 946
 947                 if (val < 0)
 948                         return val;
 949
 950                 if (val & ESTATUS_1000_TFULL)
 951                         features |= SUPPORTED_1000baseT_Full;
 952                 if (val & ESTATUS_1000_THALF)
 953                         features |= SUPPORTED_1000baseT_Half;
 954         }
 955
 956         phydev->supported = features;
 957         phydev->advertising = features;
 958
 959         return 0;
 960 }
 961 int genphy_suspend(struct phy_device *phydev)
 962 {
 963         int value;
 964
 965         mutex_lock(&phydev->lock);
 966
 967         value = phy_read(phydev, MII_BMCR);
 968         phy_write(phydev, MII_BMCR, (value | BMCR_PDOWN));
 969
 970         mutex_unlock(&phydev->lock);
 971
 972         return 0;
 973 }
 974 EXPORT_SYMBOL(genphy_suspend);
 975
 976 int genphy_resume(struct phy_device *phydev)
 977 {
 978         int value;
 979
 980         mutex_lock(&phydev->lock);
 981
 982         value = phy_read(phydev, MII_BMCR);
 983         phy_write(phydev, MII_BMCR, (value & ~BMCR_PDOWN));
 984
 985         mutex_unlock(&phydev->lock);
 986
 987         return 0;
 988 }
 989 EXPORT_SYMBOL(genphy_resume);
 990
 991 /**
 992  * phy_probe - probe and init a PHY device
 993  * @dev: device to probe and init
 994  *
 995  * Description: Take care of setting up the phy_device structure,
 996  *   set the state to READY (the driver's init function should
 997  *   set it to STARTING if needed).
 998  */
 999 static int phy_probe(struct device *dev)
1000 {
1001         struct phy_device *phydev;
1002         struct phy_driver *phydrv;
1003         struct device_driver *drv;
1004         int err = 0;
1005
1006         phydev = to_phy_device(dev);
1007
1008         drv = phydev->dev.driver;
1009         phydrv = to_phy_driver(drv);
1010         phydev->drv = phydrv;
1011
1012         /* Disable the interrupt if the PHY doesn't support it */
1013         if (!(phydrv->flags & PHY_HAS_INTERRUPT))
1014                 phydev->irq = PHY_POLL;
1015
1016         mutex_lock(&phydev->lock);
1017
1018         /* Start out supporting everything. Eventually,
1019          * a controller will attach, and may modify one
1020          * or both of these values */
1021         phydev->supported = phydrv->features;
1022         phydev->advertising = phydrv->features;
1023
1024         /* Set the state to READY by default */
1025         phydev->state = PHY_READY;
1026
1027         if (phydev->drv->probe)
1028                 err = phydev->drv->probe(phydev);
1029
1030         mutex_unlock(&phydev->lock);
1031
1032         return err;
1033
1034 }
1035
1036 static int phy_remove(struct device *dev)
1037 {
1038         struct phy_device *phydev;
1039
1040         phydev = to_phy_device(dev);
1041
1042         mutex_lock(&phydev->lock);
1043         phydev->state = PHY_DOWN;
1044         mutex_unlock(&phydev->lock);
1045
1046         if (phydev->drv->remove)
1047                 phydev->drv->remove(phydev);
1048         phydev->drv = NULL;
1049
1050         return 0;
1051 }
1052
1053 /**
1054  * phy_driver_register - register a phy_driver with the PHY layer
1055  * @new_driver: new phy_driver to register
1056  */
1057 int phy_driver_register(struct phy_driver *new_driver)
1058 {
1059         int retval;
1060
1061         new_driver->driver.name = new_driver->name;
1062         new_driver->driver.bus = &mdio_bus_type;
1063         new_driver->driver.probe = phy_probe;
1064         new_driver->driver.remove = phy_remove;
1065
1066         retval = driver_register(&new_driver->driver);
1067
1068         if (retval) {
1069                 pr_err("%s: Error %d in registering driver\n",
1070                        new_driver->name, retval);
1071
1072                 return retval;
1073         }
1074
1075         pr_debug("%s: Registered new driver\n", new_driver->name);
1076
1077         return 0;
1078 }
1079 EXPORT_SYMBOL(phy_driver_register);
1080
1081 int phy_drivers_register(struct phy_driver *new_driver, int n)
1082 {
1083         int i, ret = 0;
1084
1085         for (i = 0; i < n; i++) {
1086                 ret = phy_driver_register(new_driver + i);
1087                 if (ret) {
1088                         while (i-- > 0)
1089                                 phy_driver_unregister(new_driver + i);
1090                         break;
1091                 }
1092         }
1093         return ret;
1094 }
1095 EXPORT_SYMBOL(phy_drivers_register);
1096
1097 void phy_driver_unregister(struct phy_driver *drv)
1098 {
1099         driver_unregister(&drv->driver);
1100 }
1101 EXPORT_SYMBOL(phy_driver_unregister);
1102
1103 void phy_drivers_unregister(struct phy_driver *drv, int n)
1104 {
1105         int i;
1106         for (i = 0; i < n; i++) {
1107                 phy_driver_unregister(drv + i);
1108         }
1109 }
1110 EXPORT_SYMBOL(phy_drivers_unregister);
1111
1112 static struct phy_driver genphy_driver = {
1113         .phy_id         = 0xffffffff,
1114         .phy_id_mask    = 0xffffffff,
1115         .name           = "Generic PHY",
1116         .config_init    = genphy_config_init,
1117         .features       = 0,
1118         .config_aneg    = genphy_config_aneg,
1119         .read_status    = genphy_read_status,
1120         .suspend        = genphy_suspend,
1121         .resume         = genphy_resume,
1122         .driver         = {.owner= THIS_MODULE, },
1123 };
1124
1125 static int __init phy_init(void)
1126 {
1127         int rc;
1128
1129         rc = mdio_bus_init();
1130         if (rc)
1131                 return rc;
1132
1133         rc = phy_driver_register(&genphy_driver);
1134         if (rc)
1135                 mdio_bus_exit();
1136
1137         return rc;
1138 }
1139
1140 static void __exit phy_exit(void)
1141 {
1142         phy_driver_unregister(&genphy_driver);
1143         mdio_bus_exit();
1144 }
1145
1146 subsys_initcall(phy_init);
1147 module_exit(phy_exit);