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