2 files changed, 145 insertions, 154 deletions

diff --git a/drivers/net/Kconfig b/drivers/net/Kconfig
index 2decc597bda7..fe113d0e9456 100644
--- a/drivers/net/Kconfig
+++ b/drivers/net/Kconfig
@@ -1659,6 +1659,7 @@ config R6040
         depends on NET_PCI && PCI
         select CRC32
         select MII
+        select PHYLIB
         help
           This is a driver for the R6040 Fast Ethernet MACs found in the
           the RDC R-321x System-on-chips.
diff --git a/drivers/net/r6040.c b/drivers/net/r6040.c
index 9a251acf5ab8..6878511afc4e 100644
--- a/drivers/net/r6040.c
+++ b/drivers/net/r6040.c
@@ -44,6 +44,7 @@
 #include <linux/io.h>
 #include <linux/irq.h>
 #include <linux/uaccess.h>
+#include <linux/phy.h>
 
 #include <asm/processor.h>
 
@@ -179,7 +180,6 @@ struct r6040_descriptor {
 
 struct r6040_private {
         spinlock_t lock;                /* driver lock */
-        struct timer_list timer;
         struct pci_dev *pdev;
         struct r6040_descriptor *rx_insert_ptr;
         struct r6040_descriptor *rx_remove_ptr;
@@ -189,13 +189,15 @@ struct r6040_private {
         struct r6040_descriptor *tx_ring;
         dma_addr_t rx_ring_dma;
         dma_addr_t tx_ring_dma;
-        u16     tx_free_desc, phy_addr, phy_mode;
+        u16     tx_free_desc, phy_addr;
         u16     mcr0, mcr1;
-        u16     switch_sig;
         struct net_device *dev;
-        struct mii_if_info mii_if;
+        struct mii_bus *mii_bus;
         struct napi_struct napi;
         void __iomem *base;
+        struct phy_device *phydev;
+        int old_link;
+        int old_duplex;
 };
 
 static char version[] __devinitdata = KERN_INFO DRV_NAME
@@ -238,20 +240,30 @@ static void r6040_phy_write(void __iomem *ioaddr, int phy_addr, int reg, u16 val
         }
 }
 
-static int r6040_mdio_read(struct net_device *dev, int mii_id, int reg)
+static int r6040_mdiobus_read(struct mii_bus *bus, int phy_addr, int reg)
 {
+        struct net_device *dev = bus->priv;
         struct r6040_private *lp = netdev_priv(dev);
         void __iomem *ioaddr = lp->base;
 
-        return (r6040_phy_read(ioaddr, lp->phy_addr, reg));
+        return r6040_phy_read(ioaddr, phy_addr, reg);
 }
 
-static void r6040_mdio_write(struct net_device *dev, int mii_id, int reg, int val)
+static int r6040_mdiobus_write(struct mii_bus *bus, int phy_addr,
+                                                int reg, u16 value)
 {
+        struct net_device *dev = bus->priv;
         struct r6040_private *lp = netdev_priv(dev);
         void __iomem *ioaddr = lp->base;
 
-        r6040_phy_write(ioaddr, lp->phy_addr, reg, val);
+        r6040_phy_write(ioaddr, phy_addr, reg, value);
+
+        return 0;
+}
+
+static int r6040_mdiobus_reset(struct mii_bus *bus)
+{
+        return 0;
 }
 
 static void r6040_free_txbufs(struct net_device *dev)
@@ -408,10 +420,9 @@ static void r6040_tx_timeout(struct net_device *dev)
         void __iomem *ioaddr = priv->base;
 
         netdev_warn(dev, "transmit timed out, int enable %4.4x "
-                "status %4.4x, PHY status %4.4x\n",
+                "status %4.4x\n",
                 ioread16(ioaddr + MIER),
-                ioread16(ioaddr + MISR),
-                r6040_mdio_read(dev, priv->mii_if.phy_id, MII_BMSR));
+                ioread16(ioaddr + MISR));
 
         dev->stats.tx_errors++;
 
@@ -463,9 +474,6 @@ static int r6040_close(struct net_device *dev)
         struct r6040_private *lp = netdev_priv(dev);
         struct pci_dev *pdev = lp->pdev;
 
-        /* deleted timer */
-        del_timer_sync(&lp->timer);
-
         spin_lock_irq(&lp->lock);
         napi_disable(&lp->napi);
         netif_stop_queue(dev);
@@ -495,64 +503,14 @@ static int r6040_close(struct net_device *dev)
         return 0;
 }
 
-/* Status of PHY CHIP */
-static int r6040_phy_mode_chk(struct net_device *dev)
-{
-        struct r6040_private *lp = netdev_priv(dev);
-        void __iomem *ioaddr = lp->base;
-        int phy_dat;
-
-        /* PHY Link Status Check */
-        phy_dat = r6040_phy_read(ioaddr, lp->phy_addr, 1);
-        if (!(phy_dat & 0x4))
-                phy_dat = 0x8000;       /* Link Failed, full duplex */
-
-        /* PHY Chip Auto-Negotiation Status */
-        phy_dat = r6040_phy_read(ioaddr, lp->phy_addr, 1);
-        if (phy_dat & 0x0020) {
-                /* Auto Negotiation Mode */
-                phy_dat = r6040_phy_read(ioaddr, lp->phy_addr, 5);
-                phy_dat &= r6040_phy_read(ioaddr, lp->phy_addr, 4);
-                if (phy_dat & 0x140)
-                        /* Force full duplex */
-                        phy_dat = 0x8000;
-                else
-                        phy_dat = 0;
-        } else {
-                /* Force Mode */
-                phy_dat = r6040_phy_read(ioaddr, lp->phy_addr, 0);
-                if (phy_dat & 0x100)
-                        phy_dat = 0x8000;
-                else
-                        phy_dat = 0x0000;
-        }
-
-        return phy_dat;
-};
-
-static void r6040_set_carrier(struct mii_if_info *mii)
-{
-        if (r6040_phy_mode_chk(mii->dev)) {
-                /* autoneg is off: Link is always assumed to be up */
-                if (!netif_carrier_ok(mii->dev))
-                        netif_carrier_on(mii->dev);
-        } else
-                r6040_phy_mode_chk(mii->dev);
-}
-
 static int r6040_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 {
         struct r6040_private *lp = netdev_priv(dev);
-        struct mii_ioctl_data *data = if_mii(rq);
-        int rc;
 
-        if (!netif_running(dev))
+        if (!lp->phydev)
                 return -EINVAL;
-        spin_lock_irq(&lp->lock);
-        rc = generic_mii_ioctl(&lp->mii_if, data, cmd, NULL);
-        spin_unlock_irq(&lp->lock);
-        r6040_set_carrier(&lp->mii_if);
-        return rc;
+
+        return phy_mii_ioctl(lp->phydev, if_mii(rq), cmd);
 }
 
 static int r6040_rx(struct net_device *dev, int limit)
@@ -751,26 +709,6 @@ static int r6040_up(struct net_device *dev)
         if (ret)
                 return ret;
 
-        /* Read the PHY ID */
-        lp->switch_sig = r6040_phy_read(ioaddr, 0, 2);
-
-        if (lp->switch_sig  == ICPLUS_PHY_ID) {
-                r6040_phy_write(ioaddr, 29, 31, 0x175C); /* Enable registers */
-                lp->phy_mode = 0x8000;
-        } else {
-                /* PHY Mode Check */
-                r6040_phy_write(ioaddr, lp->phy_addr, 4, PHY_CAP);
-                r6040_phy_write(ioaddr, lp->phy_addr, 0, PHY_MODE);
-
-                if (PHY_MODE == 0x3100)
-                        lp->phy_mode = r6040_phy_mode_chk(dev);
-                else
-                        lp->phy_mode = (PHY_MODE & 0x0100) ? 0x8000:0x0;
-        }
-
-        /* Set duplex mode */
-        lp->mcr0 |= lp->phy_mode;
-
         /* improve performance (by RDC guys) */
         r6040_phy_write(ioaddr, 30, 17, (r6040_phy_read(ioaddr, 30, 17) | 0x4000));
         r6040_phy_write(ioaddr, 30, 17, ~((~r6040_phy_read(ioaddr, 30, 17)) | 0x2000));
@@ -783,35 +721,6 @@ static int r6040_up(struct net_device *dev)
         return 0;
 }
 
-/*
-  A periodic timer routine
-        Polling PHY Chip Link Status
-*/
-static void r6040_timer(unsigned long data)
-{
-        struct net_device *dev = (struct net_device *)data;
-        struct r6040_private *lp = netdev_priv(dev);
-        void __iomem *ioaddr = lp->base;
-        u16 phy_mode;
-
-        /* Polling PHY Chip Status */
-        if (PHY_MODE == 0x3100)
-                phy_mode = r6040_phy_mode_chk(dev);
-        else
-                phy_mode = (PHY_MODE & 0x0100) ? 0x8000:0x0;
-
-        if (phy_mode != lp->phy_mode) {
-                lp->phy_mode = phy_mode;
-                lp->mcr0 = (lp->mcr0 & 0x7fff) | phy_mode;
-                iowrite16(lp->mcr0, ioaddr);
-        }
-
-        /* Timer active again */
-        mod_timer(&lp->timer, round_jiffies(jiffies + HZ));
-
-        /* Check media */
-        mii_check_media(&lp->mii_if, 1, 1);
-}
 
 /* Read/set MAC address routines */
 static void r6040_mac_address(struct net_device *dev)
@@ -873,10 +782,6 @@ static int r6040_open(struct net_device *dev)
         napi_enable(&lp->napi);
         netif_start_queue(dev);
 
-        /* set and active a timer process */
-        setup_timer(&lp->timer, r6040_timer, (unsigned long) dev);
-        if (lp->switch_sig != ICPLUS_PHY_ID)
-                mod_timer(&lp->timer, jiffies + HZ);
         return 0;
 }
 
@@ -1015,40 +920,22 @@ static void netdev_get_drvinfo(struct net_device *dev,
 static int netdev_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 {
         struct r6040_private *rp = netdev_priv(dev);
-        int rc;
-
-        spin_lock_irq(&rp->lock);
-        rc = mii_ethtool_gset(&rp->mii_if, cmd);
-        spin_unlock_irq(&rp->lock);
 
-        return rc;
+        return  phy_ethtool_gset(rp->phydev, cmd);
 }
 
 static int netdev_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 {
         struct r6040_private *rp = netdev_priv(dev);
-        int rc;
-
-        spin_lock_irq(&rp->lock);
-        rc = mii_ethtool_sset(&rp->mii_if, cmd);
-        spin_unlock_irq(&rp->lock);
-        r6040_set_carrier(&rp->mii_if);
-
-        return rc;
-}
-
-static u32 netdev_get_link(struct net_device *dev)
-{
-        struct r6040_private *rp = netdev_priv(dev);
 
-        return mii_link_ok(&rp->mii_if);
+        return phy_ethtool_sset(rp->phydev, cmd);
 }
 
 static const struct ethtool_ops netdev_ethtool_ops = {
         .get_drvinfo            = netdev_get_drvinfo,
         .get_settings           = netdev_get_settings,
         .set_settings           = netdev_set_settings,
-        .get_link               = netdev_get_link,
+        .get_link               = ethtool_op_get_link,
 };
 
 static const struct net_device_ops r6040_netdev_ops = {
@@ -1067,6 +954,79 @@ static const struct net_device_ops r6040_netdev_ops = {
 #endif
 };
 
+static void r6040_adjust_link(struct net_device *dev)
+{
+        struct r6040_private *lp = netdev_priv(dev);
+        struct phy_device *phydev = lp->phydev;
+        int status_changed = 0;
+        void __iomem *ioaddr = lp->base;
+
+        BUG_ON(!phydev);
+
+        if (lp->old_link != phydev->link) {
+                status_changed = 1;
+                lp->old_link = phydev->link;
+        }
+
+        /* reflect duplex change */
+        if (phydev->link && (lp->old_duplex != phydev->duplex)) {
+                lp->mcr0 |= (phydev->duplex == DUPLEX_FULL ? 0x8000 : 0);
+                iowrite16(lp->mcr0, ioaddr);
+
+                status_changed = 1;
+                lp->old_duplex = phydev->duplex;
+        }
+
+        if (status_changed) {
+                pr_info("%s: link %s", dev->name, phydev->link ?
+                        "UP" : "DOWN");
+                if (phydev->link)
+                        pr_cont(" - %d/%s", phydev->speed,
+                        DUPLEX_FULL == phydev->duplex ? "full" : "half");
+                pr_cont("\n");
+        }
+}
+
+static int r6040_mii_probe(struct net_device *dev)
+{
+        struct r6040_private *lp = netdev_priv(dev);
+        struct phy_device *phydev = NULL;
+
+        phydev = phy_find_first(lp->mii_bus);
+        if (!phydev) {
+                dev_err(&lp->pdev->dev, "no PHY found\n");
+                return -ENODEV;
+        }
+
+        phydev = phy_connect(dev, dev_name(&phydev->dev), &r6040_adjust_link,
+                                0, PHY_INTERFACE_MODE_MII);
+
+        if (IS_ERR(phydev)) {
+                dev_err(&lp->pdev->dev, "could not attach to PHY\n");
+                return PTR_ERR(phydev);
+        }
+
+        /* mask with MAC supported features */
+        phydev->supported &= (SUPPORTED_10baseT_Half
+                                | SUPPORTED_10baseT_Full
+                                | SUPPORTED_100baseT_Half
+                                | SUPPORTED_100baseT_Full
+                                | SUPPORTED_Autoneg
+                                | SUPPORTED_MII
+                                | SUPPORTED_TP);
+
+        phydev->advertising = phydev->supported;
+        lp->phydev = phydev;
+        lp->old_link = 0;
+        lp->old_duplex = -1;
+
+        dev_info(&lp->pdev->dev, "attached PHY driver [%s] "
+                "(mii_bus:phy_addr=%s)\n",
+                phydev->drv->name, dev_name(&phydev->dev));
+
+        return 0;
+}
+
 static int __devinit r6040_init_one(struct pci_dev *pdev,
                                          const struct pci_device_id *ent)
 {
@@ -1077,6 +1037,7 @@ static int __devinit r6040_init_one(struct pci_dev *pdev,
         static int card_idx = -1;
         int bar = 0;
         u16 *adrp;
+        int i;
 
         printk("%s\n", version);
 
@@ -1163,7 +1124,6 @@ static int __devinit r6040_init_one(struct pci_dev *pdev,
         /* Init RDC private data */
         lp->mcr0 = 0x1002;
         lp->phy_addr = phy_table[card_idx];
-        lp->switch_sig = 0;
 
         /* The RDC-specific entries in the device structure. */
         dev->netdev_ops = &r6040_netdev_ops;
@@ -1171,28 +1131,54 @@ static int __devinit r6040_init_one(struct pci_dev *pdev,
         dev->watchdog_timeo = TX_TIMEOUT;
 
         netif_napi_add(dev, &lp->napi, r6040_poll, 64);
-        lp->mii_if.dev = dev;
-        lp->mii_if.mdio_read = r6040_mdio_read;
-        lp->mii_if.mdio_write = r6040_mdio_write;
-        lp->mii_if.phy_id = lp->phy_addr;
-        lp->mii_if.phy_id_mask = 0x1f;
-        lp->mii_if.reg_num_mask = 0x1f;
-
-        /* Check the vendor ID on the PHY, if 0xffff assume none attached */
-        if (r6040_phy_read(ioaddr, lp->phy_addr, 2) == 0xffff) {
-                dev_err(&pdev->dev, "Failed to detect an attached PHY\n");
-                err = -ENODEV;
+
+        lp->mii_bus = mdiobus_alloc();
+        if (!lp->mii_bus) {
+                dev_err(&pdev->dev, "mdiobus_alloc() failed\n");
                 goto err_out_unmap;
         }
 
+        lp->mii_bus->priv = dev;
+        lp->mii_bus->read = r6040_mdiobus_read;
+        lp->mii_bus->write = r6040_mdiobus_write;
+        lp->mii_bus->reset = r6040_mdiobus_reset;
+        lp->mii_bus->name = "r6040_eth_mii";
+        snprintf(lp->mii_bus->id, MII_BUS_ID_SIZE, "%x", card_idx);
+        lp->mii_bus->irq = kmalloc(sizeof(int)*PHY_MAX_ADDR, GFP_KERNEL);
+        if (!lp->mii_bus->irq) {
+                dev_err(&pdev->dev, "mii_bus irq allocation failed\n");
+                goto err_out_mdio;
+        }
+
+        for (i = 0; i < PHY_MAX_ADDR; i++)
+                lp->mii_bus->irq[i] = PHY_POLL;
+
+        err = mdiobus_register(lp->mii_bus);
+        if (err) {
+                dev_err(&pdev->dev, "failed to register MII bus\n");
+                goto err_out_mdio_irq;
+        }
+
+        err = r6040_mii_probe(dev);
+        if (err) {
+                dev_err(&pdev->dev, "failed to probe MII bus\n");
+                goto err_out_mdio_unregister;
+        }
+
         /* Register net device. After this dev->name assign */
         err = register_netdev(dev);
         if (err) {
                 dev_err(&pdev->dev, "Failed to register net device\n");
-                goto err_out_unmap;
+                goto err_out_mdio_unregister;
         }
         return 0;
 
+err_out_mdio_unregister:
+        mdiobus_unregister(lp->mii_bus);
+err_out_mdio_irq:
+        kfree(lp->mii_bus->irq);
+err_out_mdio:
+        mdiobus_free(lp->mii_bus);
 err_out_unmap:
         pci_iounmap(pdev, ioaddr);
 err_out_free_res:
@@ -1206,8 +1192,12 @@ err_out:
 static void __devexit r6040_remove_one(struct pci_dev *pdev)
 {
         struct net_device *dev = pci_get_drvdata(pdev);
+        struct r6040_private *lp = netdev_priv(dev);
 
         unregister_netdev(dev);
+        mdiobus_unregister(lp->mii_bus);
+        kfree(lp->mii_bus->irq);
+        mdiobus_free(lp->mii_bus);
         pci_release_regions(pdev);
         free_netdev(dev);
         pci_disable_device(pdev);