net: introduce phylib

Adapt phylib from linux

switch all the driver to it
reimplement mii bus

This will allow to have
 - phy drivers
 - to only connect the phy at then opening of the device
 - if the phy is not ready or not up fail on open

Same behaviour as in linux and will allow to share code and simplify porting.

Signed-off-by: Jean-Christophe PLAGNIOL-VILLARD <plagnioj@jcrosoft.com>

5 files changed, 873 insertions, 0 deletions

diff --git a/drivers/net/phy/Kconfig b/drivers/net/phy/Kconfig
new file mode 100644
index 0000000000..b66261ae99
--- /dev/null
+++ b/drivers/net/phy/Kconfig
@@ -0,0 +1,17 @@
+#
+# PHY Layer Configuration
+#
+
+menu "phylib                       "
+
+if PHYLIB
+
+comment "MII PHY device drivers"
+
+config GENERIC_PHY
+	bool "Drivers for the Generic PHYs"
+	default y
+
+endif
+
+endmenu
diff --git a/drivers/net/phy/Makefile b/drivers/net/phy/Makefile
new file mode 100644
index 0000000000..82e90d426a
--- /dev/null
+++ b/drivers/net/phy/Makefile
@@ -0,0 +1,2 @@
+obj-y += phy.o mdio_bus.o
+obj-$(CONFIG_GENERIC_PHY) += generic.o
diff --git a/drivers/net/phy/generic.c b/drivers/net/phy/generic.c
new file mode 100644
index 0000000000..3f5f127065
--- /dev/null
+++ b/drivers/net/phy/generic.c
@@ -0,0 +1,36 @@
+/*
+ * Copyright (c) 2009 Jean-Christophe PLAGNIOL-VILLARD <plagnioj@jcrosoft.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ *
+ */
+
+#include <common.h>
+#include <linux/phy.h>
+#include <init.h>
+
+static struct phy_driver generic_phy = {
+	.drv.name = "Generic PHY",
+	.phy_id = PHY_ANY_UID,
+	.phy_id_mask = PHY_ANY_UID,
+	.features = 0,
+};
+
+static int generic_phy_register(void)
+{
+	return phy_driver_register(&generic_phy);
+}
+device_initcall(generic_phy_register);
diff --git a/drivers/net/phy/mdio_bus.c b/drivers/net/phy/mdio_bus.c
new file mode 100644
index 0000000000..93d6fe15e1
--- /dev/null
+++ b/drivers/net/phy/mdio_bus.c
@@ -0,0 +1,250 @@
+/*
+ * drivers/net/phy/mdio_bus.c
+ *
+ * MDIO Bus interface
+ *
+ * Author: Andy Fleming
+ *
+ * Copyright (c) 2004 Freescale Semiconductor, Inc.
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <common.h>
+#include <driver.h>
+#include <init.h>
+#include <clock.h>
+#include <net.h>
+#include <errno.h>
+#include <linux/phy.h>
+#include <linux/err.h>
+
+/**
+ * mdiobus_register - bring up all the PHYs on a given bus and attach them to bus
+ * @bus: target mii_bus
+ *
+ * Description: Called by a bus driver to bring up all the PHYs
+ *   on a given bus, and attach them to the bus.
+ *
+ * Returns 0 on success or < 0 on error.
+ */
+int mdiobus_register(struct mii_bus *bus)
+{
+	int i, err;
+
+	if (NULL == bus ||
+			NULL == bus->read ||
+			NULL == bus->write)
+		return -EINVAL;
+
+	bus->dev.priv = bus;
+	bus->dev.id = DEVICE_ID_DYNAMIC;
+	strcpy(bus->dev.name, "miibus");
+	bus->dev.parent = bus->parent;
+	if (bus->parent)
+		dev_add_child(bus->parent, &bus->dev);
+
+	err = register_device(&bus->dev);
+	if (err) {
+		pr_err("mii_bus %s failed to register\n", bus->dev.name);
+		return -EINVAL;
+	}
+
+	if (bus->reset)
+		bus->reset(bus);
+
+	for (i = 0; i < PHY_MAX_ADDR; i++) {
+		if ((bus->phy_mask & (1 << i)) == 0) {
+			struct phy_device *phydev;
+
+			phydev = mdiobus_scan(bus, i);
+			if (IS_ERR(phydev)) {
+				err = PTR_ERR(phydev);
+				goto error;
+			}
+		}
+	}
+
+	pr_info("%s: probed\n", dev_name(&bus->dev));
+	return 0;
+
+error:
+	while (--i >= 0) {
+		if (bus->phy_map[i]) {
+			kfree(bus->phy_map[i]);
+			bus->phy_map[i] = NULL;
+		}
+	}
+	return err;
+}
+EXPORT_SYMBOL(mdiobus_register);
+
+void mdiobus_unregister(struct mii_bus *bus)
+{
+	int i;
+
+	for (i = 0; i < PHY_MAX_ADDR; i++) {
+		if (bus->phy_map[i])
+			unregister_device(&bus->phy_map[i]->dev);
+		bus->phy_map[i] = NULL;
+	}
+}
+EXPORT_SYMBOL(mdiobus_unregister);
+
+struct phy_device *mdiobus_scan(struct mii_bus *bus, int addr)
+{
+	struct phy_device *phydev;
+
+	phydev = get_phy_device(bus, addr);
+	if (IS_ERR(phydev) || phydev == NULL)
+		return phydev;
+
+	bus->phy_map[addr] = phydev;
+
+	return phydev;
+}
+EXPORT_SYMBOL(mdiobus_scan);
+
+/**
+ * mdio_bus_match - determine if given PHY driver supports the given PHY device
+ * @dev: target PHY device
+ * @drv: given PHY driver
+ *
+ * Description: Given a PHY device, and a PHY driver, return 1 if
+ *   the driver supports the device.  Otherwise, return 0.
+ */
+static int mdio_bus_match(struct device_d *dev, struct driver_d *drv)
+{
+	struct phy_device *phydev = to_phy_device(dev);
+	struct phy_driver *phydrv = to_phy_driver(drv);
+
+	return ((phydrv->phy_id & phydrv->phy_id_mask) ==
+		(phydev->phy_id & phydrv->phy_id_mask));
+}
+
+static ssize_t phydev_read(struct cdev *cdev, void *_buf, size_t count, loff_t offset, ulong flags)
+{
+	int i = count;
+	uint16_t *buf = _buf;
+	struct phy_device *phydev = cdev->priv;
+
+	while (i > 0) {
+		*buf = phy_read(phydev, offset / 2);
+		buf++;
+		i -= 2;
+		offset += 2;
+	}
+
+	return count;
+}
+
+static ssize_t phydev_write(struct cdev *cdev, const void *_buf, size_t count, loff_t offset, ulong flags)
+{
+	int i = count;
+	const uint16_t *buf = _buf;
+	struct phy_device *phydev = cdev->priv;
+
+	while (i > 0) {
+		phy_write(phydev, offset / 2, *buf);
+		buf++;
+		i -= 2;
+		offset += 2;
+	}
+
+	return count;
+}
+
+static struct file_operations phydev_ops = {
+	.read  = phydev_read,
+	.write = phydev_write,
+	.lseek = dev_lseek_default,
+};
+
+static int mdio_bus_probe(struct device_d *_dev)
+{
+	struct phy_device *dev = to_phy_device(_dev);
+	struct phy_driver *drv = to_phy_driver(_dev->driver);
+
+	char str[16];
+
+	dev->attached_dev->phydev = dev;
+	dev->dev.parent = &dev->attached_dev->dev;
+	dev_add_child(dev->dev.parent, _dev);
+
+	if (drv->probe) {
+		int ret;
+
+		ret = drv->probe(dev);
+		if (ret) {
+			dev->attached_dev->phydev = NULL;
+			dev->attached_dev = NULL;
+			return ret;
+		}
+	}
+
+	if (dev->dev_flags) {
+		if (dev->dev_flags & PHYLIB_FORCE_10) {
+			dev->speed = SPEED_10;
+			dev->duplex = DUPLEX_FULL;
+			dev->autoneg = !AUTONEG_ENABLE;
+		}
+	}
+
+	/* Start out supporting everything. Eventually,
+	 * a controller will attach, and may modify one
+	 * or both of these values */
+	dev->supported = drv->features;
+	dev->advertising = drv->features;
+
+	drv->config_init(dev);
+
+	/* Sanitize settings based on PHY capabilities */
+	if ((dev->supported & SUPPORTED_Autoneg) == 0)
+		dev->autoneg = AUTONEG_DISABLE;
+
+	sprintf(str, "%d", dev->addr);
+	dev_add_param_fixed(&dev->dev, "phy_addr", str);
+
+	dev->cdev.name = asprintf("phy%d", _dev->id);
+	dev->cdev.size = 64;
+	dev->cdev.ops = &phydev_ops;
+	dev->cdev.priv = dev;
+	dev->cdev.dev = _dev;
+	devfs_create(&dev->cdev);
+
+	return 0;
+}
+
+static void mdio_bus_remove(struct device_d *_dev)
+{
+	struct phy_device *dev = to_phy_device(_dev);
+	struct phy_driver *drv = to_phy_driver(_dev->driver);
+
+	if (drv->remove)
+		drv->remove(dev);
+
+	free(dev->cdev.name);
+	devfs_remove(&dev->cdev);
+}
+
+struct bus_type mdio_bus_type = {
+	.name		= "mdio_bus",
+	.match		= mdio_bus_match,
+	.probe		= mdio_bus_probe,
+	.remove		= mdio_bus_remove,
+};
+EXPORT_SYMBOL(mdio_bus_type);
+
+#if 0
+static int mdio_bus_init(void)
+{
+	return bus_register(&mdio_bus_type);
+}
+pure_initcall(mdio_bus_init);
+#endif
diff --git a/drivers/net/phy/phy.c b/drivers/net/phy/phy.c
new file mode 100644
index 0000000000..e1a24faf92
--- /dev/null
+++ b/drivers/net/phy/phy.c
@@ -0,0 +1,568 @@
+/*
+ * drivers/net/phy/phy.c
+ *
+ * Framework for finding and configuring PHYs.
+ * Also contains generic PHY driver
+ *
+ * Copyright (c) 2009-2012 Jean-Christophe PLAGNIOL-VILLARD <plagnioj@jcrosoft.com>
+ *
+ * Author: Andy Fleming
+ *
+ * Copyright (c) 2004 Freescale Semiconductor, Inc.
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ */
+
+#include <common.h>
+#include <driver.h>
+#include <net.h>
+#include <malloc.h>
+#include <linux/phy.h>
+#include <linux/phy.h>
+#include <linux/err.h>
+
+#define PHY_AN_TIMEOUT	10
+
+static int genphy_config_init(struct phy_device *phydev);
+
+struct phy_device *phy_device_create(struct mii_bus *bus, int addr, int phy_id)
+{
+	struct phy_device *dev;
+
+	/* We allocate the device, and initialize the
+	 * default values */
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+
+	if (NULL == dev)
+		return (struct phy_device*) PTR_ERR((void*)-ENOMEM);
+
+	dev->speed = 0;
+	dev->duplex = -1;
+	dev->pause = dev->asym_pause = 0;
+	dev->link = 1;
+	dev->autoneg = AUTONEG_ENABLE;
+
+	dev->addr = addr;
+	dev->phy_id = phy_id;
+
+	dev->bus = bus;
+	dev->dev.parent = bus->parent;
+	dev->dev.bus = &mdio_bus_type;
+
+	strcpy(dev->dev.name, "phy");
+	dev->dev.id = DEVICE_ID_DYNAMIC;
+
+	return dev;
+}
+/**
+ * get_phy_id - reads the specified addr for its ID.
+ * @bus: the target MII bus
+ * @addr: PHY address on the MII bus
+ * @phy_id: where to store the ID retrieved.
+ *
+ * Description: Reads the ID registers of the PHY at @addr on the
+ *   @bus, stores it in @phy_id and returns zero on success.
+ */
+int get_phy_id(struct mii_bus *bus, int addr, u32 *phy_id)
+{
+	int phy_reg;
+
+	/* Grab the bits from PHYIR1, and put them
+	 * in the upper half */
+	phy_reg = mdiobus_read(bus, addr, MII_PHYSID1);
+
+	if (phy_reg < 0)
+		return -EIO;
+
+	*phy_id = (phy_reg & 0xffff) << 16;
+
+	/* Grab the bits from PHYIR2, and put them in the lower half */
+	phy_reg = mdiobus_read(bus, addr, MII_PHYSID2);
+
+	if (phy_reg < 0)
+		return -EIO;
+
+	*phy_id |= (phy_reg & 0xffff);
+
+	return 0;
+}
+
+/**
+ * get_phy_device - reads the specified PHY device and returns its @phy_device struct
+ * @bus: the target MII bus
+ * @addr: PHY address on the MII bus
+ *
+ * Description: Reads the ID registers of the PHY at @addr on the
+ *   @bus, then allocates and returns the phy_device to represent it.
+ */
+struct phy_device *get_phy_device(struct mii_bus *bus, int addr)
+{
+	struct phy_device *dev = NULL;
+	u32 phy_id = 0;
+	int r;
+
+	r = get_phy_id(bus, addr, &phy_id);
+	if (r)
+		return ERR_PTR(r);
+
+	/* If the phy_id is mostly Fs, there is no device there */
+	if ((phy_id & 0x1fffffff) == 0x1fffffff)
+		return NULL;
+
+	dev = phy_device_create(bus, addr, phy_id);
+
+	return dev;
+}
+
+/* Automatically gets and returns the PHY device */
+int phy_device_connect(struct eth_device *edev, struct mii_bus *bus, int addr,
+		       void (*adjust_link) (struct eth_device *edev),
+		       u32 flags, phy_interface_t interface)
+{
+	struct phy_driver* drv;
+	struct phy_device* dev = NULL;
+	unsigned int i;
+	int ret = -EINVAL;
+
+	if (!edev->phydev) {
+		if (addr >= 0) {
+			dev = bus->phy_map[addr];
+			if (!dev) {
+				ret = -EIO;
+				goto fail;
+			}
+
+			dev->attached_dev = edev;
+			dev->interface = interface;
+			dev->dev_flags = flags;
+
+			ret = register_device(&dev->dev);
+			if (ret)
+				goto fail;
+		} else {
+			for (i = 0; i < PHY_MAX_ADDR && !edev->phydev; i++) {
+				dev = bus->phy_map[i];
+				if (!dev || dev->attached_dev)
+					continue;
+
+				dev->attached_dev = edev;
+				dev->interface = interface;
+				dev->dev_flags = flags;
+
+				ret = register_device(&dev->dev);
+				if (ret)
+					goto fail;
+
+				break;
+			}
+		}
+
+		if (!edev->phydev) {
+			ret = -EIO;
+			goto fail;
+		}
+	}
+
+	dev = edev->phydev;
+	drv = to_phy_driver(dev->dev.driver);
+
+	drv->config_aneg(dev);
+
+	ret = drv->read_status(dev);
+	if (ret < 0)
+		return ret;
+
+	if (dev->link)
+		printf("%dMbps %s duplex link detected\n", dev->speed,
+			dev->duplex ? "full" : "half");
+
+	if (adjust_link)
+		adjust_link(edev);
+
+	return 0;
+
+fail:
+	if (dev)
+		dev->attached_dev = NULL;
+	puts("Unable to find a PHY (unknown ID?)\n");
+	return ret;
+}
+
+/* Generic PHY support and helper functions */
+
+/**
+ * genphy_config_advert - sanitize and advertise auto-negotiation parameters
+ * @phydev: target phy_device struct
+ *
+ * Description: Writes MII_ADVERTISE with the appropriate values,
+ *   after sanitizing the values to make sure we only advertise
+ *   what is supported.  Returns < 0 on error, 0 if the PHY's advertisement
+ *   hasn't changed, and > 0 if it has changed.
+ */
+int genphy_config_advert(struct phy_device *phydev)
+{
+	u32 advertise;
+	int oldadv, adv;
+	int err, changed = 0;
+
+	/* Only allow advertising what
+	 * this PHY supports */
+	phydev->advertising &= phydev->supported;
+	advertise = phydev->advertising;
+
+	/* Setup standard advertisement */
+	oldadv = adv = phy_read(phydev, MII_ADVERTISE);
+
+	if (adv < 0)
+		return adv;
+
+	adv &= ~(ADVERTISE_ALL | ADVERTISE_100BASE4 | ADVERTISE_PAUSE_CAP |
+		 ADVERTISE_PAUSE_ASYM);
+	adv |= ethtool_adv_to_mii_adv_t(advertise);
+
+	if (adv != oldadv) {
+		err = phy_write(phydev, MII_ADVERTISE, adv);
+
+		if (err < 0)
+			return err;
+		changed = 1;
+	}
+
+	/* Configure gigabit if it's supported */
+	if (phydev->supported & (SUPPORTED_1000baseT_Half |
+				SUPPORTED_1000baseT_Full)) {
+		oldadv = adv = phy_read(phydev, MII_CTRL1000);
+
+		if (adv < 0)
+			return adv;
+
+		adv &= ~(ADVERTISE_1000FULL | ADVERTISE_1000HALF);
+		adv |= ethtool_adv_to_mii_ctrl1000_t(advertise);
+
+		if (adv != oldadv) {
+			err = phy_write(phydev, MII_CTRL1000, adv);
+
+			if (err < 0)
+				return err;
+			changed = 1;
+		}
+	}
+
+	return changed;
+}
+
+/**
+ * genphy_setup_forced - configures/forces speed/duplex from @phydev
+ * @phydev: target phy_device struct
+ *
+ * Description: Configures MII_BMCR to force speed/duplex
+ *   to the values in phydev. Assumes that the values are valid.
+ *   Please see phy_sanitize_settings().
+ */
+int genphy_setup_forced(struct phy_device *phydev)
+{
+	int err;
+	int ctl = 0;
+
+	phydev->pause = phydev->asym_pause = 0;
+
+	if (SPEED_1000 == phydev->speed)
+		ctl |= BMCR_SPEED1000;
+	else if (SPEED_100 == phydev->speed)
+		ctl |= BMCR_SPEED100;
+
+	if (DUPLEX_FULL == phydev->duplex)
+		ctl |= BMCR_FULLDPLX;
+
+	err = phy_write(phydev, MII_BMCR, ctl);
+
+	return err;
+}
+
+static int phy_aneg_done(struct phy_device *phydev)
+{
+	uint64_t start = get_time_ns();
+	int ctl;
+
+	while (!is_timeout(start, PHY_AN_TIMEOUT * SECOND)) {
+		ctl = phy_read(phydev, MII_BMSR);
+		if (ctl & BMSR_ANEGCOMPLETE) {
+			phydev->link = 1;
+			return 0;
+		}
+
+		/* Restart auto-negotiation if remote fault */
+		if (ctl & BMSR_RFAULT) {
+			puts("PHY remote fault detected\n"
+			     "PHY restarting auto-negotiation\n");
+			phy_write(phydev, MII_BMCR,
+					  BMCR_ANENABLE | BMCR_ANRESTART);
+		}
+	}
+
+	phydev->link = 0;
+	return -ETIMEDOUT;
+}
+
+/**
+ * genphy_restart_aneg - Enable and Restart Autonegotiation
+ * @phydev: target phy_device struct
+ */
+int genphy_restart_aneg(struct phy_device *phydev)
+{
+	int ctl;
+
+	ctl = phy_read(phydev, MII_BMCR);
+
+	if (ctl < 0)
+		return ctl;
+
+	ctl |= (BMCR_ANENABLE | BMCR_ANRESTART);
+
+	/* Don't isolate the PHY if we're negotiating */
+	ctl &= ~(BMCR_ISOLATE);
+
+	ctl = phy_write(phydev, MII_BMCR, ctl);
+
+	if (ctl < 0)
+		return ctl;
+
+	return phy_aneg_done(phydev);
+}
+
+/**
+ * genphy_config_aneg - restart auto-negotiation or write BMCR
+ * @phydev: target phy_device struct
+ *
+ * Description: If auto-negotiation is enabled, we configure the
+ *   advertising, and then restart auto-negotiation.  If it is not
+ *   enabled, then we write the BMCR.
+ */
+int genphy_config_aneg(struct phy_device *phydev)
+{
+	int result;
+
+	if (AUTONEG_ENABLE != phydev->autoneg)
+		return genphy_setup_forced(phydev);
+
+	result = genphy_config_advert(phydev);
+
+	if (result < 0) /* error */
+		return result;
+
+	if (result == 0) {
+		/* Advertisement hasn't changed, but maybe aneg was never on to
+		 * begin with?  Or maybe phy was isolated? */
+		int ctl = phy_read(phydev, MII_BMCR);
+
+		if (ctl < 0)
+			return ctl;
+
+		if (!(ctl & BMCR_ANENABLE) || (ctl & BMCR_ISOLATE))
+			result = 1; /* do restart aneg */
+	}
+
+	/* Only restart aneg if we are advertising something different
+	 * than we were before.	 */
+	if (result > 0)
+		result = genphy_restart_aneg(phydev);
+
+	return result;
+}
+
+/**
+ * genphy_update_link - update link status in @phydev
+ * @phydev: target phy_device struct
+ *
+ * Description: Update the value in phydev->link to reflect the
+ *   current link value.  In order to do this, we need to read
+ *   the status register twice, keeping the second value.
+ */
+int genphy_update_link(struct phy_device *phydev)
+{
+	int status;
+
+	/* Do a fake read */
+	status = phy_read(phydev, MII_BMSR);
+
+	if (status < 0)
+		return status;
+
+	/* wait phy status update in the phy */
+	udelay(1000);
+
+	/* Read link and autonegotiation status */
+	status = phy_read(phydev, MII_BMSR);
+
+	if (status < 0)
+		return status;
+
+	if ((status & BMSR_LSTATUS) == 0)
+		phydev->link = 0;
+	else
+		phydev->link = 1;
+
+	return 0;
+}
+
+/**
+ * genphy_read_status - check the link status and update current link state
+ * @phydev: target phy_device struct
+ *
+ * Description: Check the link, then figure out the current state
+ *   by comparing what we advertise with what the link partner
+ *   advertises.  Start by checking the gigabit possibilities,
+ *   then move on to 10/100.
+ */
+int genphy_read_status(struct phy_device *phydev)
+{
+	int adv;
+	int err;
+	int lpa;
+	int lpagb = 0;
+
+	/* Update the link, but return if there
+	 * was an error */
+	err = genphy_update_link(phydev);
+	if (err)
+		return err;
+
+	if (AUTONEG_ENABLE == phydev->autoneg) {
+		if (phydev->supported & (SUPPORTED_1000baseT_Half
+					| SUPPORTED_1000baseT_Full)) {
+			lpagb = phy_read(phydev, MII_STAT1000);
+
+			if (lpagb < 0)
+				return lpagb;
+
+			adv = phy_read(phydev, MII_CTRL1000);
+
+			if (adv < 0)
+				return adv;
+
+			lpagb &= adv << 2;
+		}
+
+		lpa = phy_read(phydev, MII_LPA);
+
+		if (lpa < 0)
+			return lpa;
+
+		adv = phy_read(phydev, MII_ADVERTISE);
+
+		if (adv < 0)
+			return adv;
+
+		lpa &= adv;
+
+		phydev->speed = SPEED_10;
+		phydev->duplex = DUPLEX_HALF;
+		phydev->pause = phydev->asym_pause = 0;
+
+		if (lpagb & (LPA_1000FULL | LPA_1000HALF)) {
+			phydev->speed = SPEED_1000;
+
+			if (lpagb & LPA_1000FULL)
+				phydev->duplex = DUPLEX_FULL;
+		} else if (lpa & (LPA_100FULL | LPA_100HALF)) {
+			phydev->speed = SPEED_100;
+
+			if (lpa & LPA_100FULL)
+				phydev->duplex = DUPLEX_FULL;
+		} else
+			if (lpa & LPA_10FULL)
+				phydev->duplex = DUPLEX_FULL;
+
+		if (phydev->duplex == DUPLEX_FULL) {
+			phydev->pause = lpa & LPA_PAUSE_CAP ? 1 : 0;
+			phydev->asym_pause = lpa & LPA_PAUSE_ASYM ? 1 : 0;
+		}
+	} else {
+		int bmcr = phy_read(phydev, MII_BMCR);
+		if (bmcr < 0)
+			return bmcr;
+
+		if (bmcr & BMCR_FULLDPLX)
+			phydev->duplex = DUPLEX_FULL;
+		else
+			phydev->duplex = DUPLEX_HALF;
+
+		if (bmcr & BMCR_SPEED1000)
+			phydev->speed = SPEED_1000;
+		else if (bmcr & BMCR_SPEED100)
+			phydev->speed = SPEED_100;
+		else
+			phydev->speed = SPEED_10;
+
+		phydev->pause = phydev->asym_pause = 0;
+	}
+
+	return 0;
+}
+
+static int genphy_config_init(struct phy_device *phydev)
+{
+	int val;
+	u32 features;
+
+	/* For now, I'll claim that the generic driver supports
+	 * all possible port types */
+	features = (SUPPORTED_TP | SUPPORTED_MII
+			| SUPPORTED_AUI | SUPPORTED_FIBRE |
+			SUPPORTED_BNC);
+
+	/* Do we support autonegotiation? */
+	val = phy_read(phydev, MII_BMSR);
+
+	if (val < 0)
+		return val;
+
+	if (val & BMSR_ANEGCAPABLE)
+		features |= SUPPORTED_Autoneg;
+
+	if (val & BMSR_100FULL)
+		features |= SUPPORTED_100baseT_Full;
+	if (val & BMSR_100HALF)
+		features |= SUPPORTED_100baseT_Half;
+	if (val & BMSR_10FULL)
+		features |= SUPPORTED_10baseT_Full;
+	if (val & BMSR_10HALF)
+		features |= SUPPORTED_10baseT_Half;
+
+	if (val & BMSR_ESTATEN) {
+		val = phy_read(phydev, MII_ESTATUS);
+
+		if (val < 0)
+			return val;
+
+		if (val & ESTATUS_1000_TFULL)
+			features |= SUPPORTED_1000baseT_Full;
+		if (val & ESTATUS_1000_THALF)
+			features |= SUPPORTED_1000baseT_Half;
+	}
+
+	phydev->supported = features;
+	phydev->advertising = features;
+
+	return 0;
+}
+
+int phy_driver_register(struct phy_driver *phydrv)
+{
+	phydrv->drv.bus = &mdio_bus_type;
+
+	if (!phydrv->config_init)
+		phydrv->config_init = genphy_config_init;
+
+	if (!phydrv->config_aneg)
+		phydrv->config_aneg = genphy_config_aneg;
+
+	if (!phydrv->read_status)
+		phydrv->read_status = genphy_read_status;
+
+	return register_driver(&phydrv->drv);
+}