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