ifb: add performance flags
[linux-2.6.git] / drivers / net / fs_enet / fs_enet-main.c
index 83c9bf2..d684f18 100644 (file)
@@ -1,17 +1,17 @@
 /*
  * Combined Ethernet driver for Motorola MPC8xx and MPC82xx.
  *
- * Copyright (c) 2003 Intracom S.A. 
+ * Copyright (c) 2003 Intracom S.A.
  *  by Pantelis Antoniou <panto@intracom.gr>
- * 
- * 2005 (c) MontaVista Software, Inc. 
+ *
+ * 2005 (c) MontaVista Software, Inc.
  * Vitaly Bordug <vbordug@ru.mvista.com>
  *
  * Heavily based on original FEC driver by Dan Malek <dan@embeddededge.com>
  * and modifications by Joakim Tjernlund <joakim.tjernlund@lumentis.se>
  *
- * This file is licensed under the terms of the GNU General Public License 
- * version 2. This program is licensed "as is" without any warranty of any 
+ * This file is licensed under the terms of the GNU General Public License
+ * version 2. This program is licensed "as is" without any warranty of any
  * kind, whether express or implied.
  */
 
 #include <linux/fs.h>
 #include <linux/platform_device.h>
 #include <linux/phy.h>
+#include <linux/of.h>
+#include <linux/of_mdio.h>
+#include <linux/of_platform.h>
+#include <linux/of_gpio.h>
 
 #include <linux/vmalloc.h>
 #include <asm/pgtable.h>
 
 /*************************************************/
 
-static char version[] __devinitdata =
-    DRV_MODULE_NAME ".c:v" DRV_MODULE_VERSION " (" DRV_MODULE_RELDATE ")" "\n";
-
 MODULE_AUTHOR("Pantelis Antoniou <panto@intracom.gr>");
 MODULE_DESCRIPTION("Freescale Ethernet Driver");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(DRV_MODULE_VERSION);
 
-int fs_enet_debug = -1;                /* -1 == use FS_ENET_DEF_MSG_ENABLE as value */
+static int fs_enet_debug = -1; /* -1 == use FS_ENET_DEF_MSG_ENABLE as value */
 module_param(fs_enet_debug, int, 0);
 MODULE_PARM_DESC(fs_enet_debug,
                 "Freescale bitmapped debugging message enable value");
 
+#ifdef CONFIG_NET_POLL_CONTROLLER
+static void fs_enet_netpoll(struct net_device *dev);
+#endif
 
 static void fs_set_multicast_list(struct net_device *dev)
 {
@@ -67,21 +71,26 @@ static void fs_set_multicast_list(struct net_device *dev)
        (*fep->ops->set_multicast_list)(dev);
 }
 
+static void skb_align(struct sk_buff *skb, int align)
+{
+       int off = ((unsigned long)skb->data) & (align - 1);
+
+       if (off)
+               skb_reserve(skb, align - off);
+}
+
 /* NAPI receive function */
 static int fs_enet_rx_napi(struct napi_struct *napi, int budget)
 {
        struct fs_enet_private *fep = container_of(napi, struct fs_enet_private, napi);
-       struct net_device *dev = to_net_dev(fep->dev);
+       struct net_device *dev = fep->ndev;
        const struct fs_platform_info *fpi = fep->fpi;
-       cbd_t *bdp;
+       cbd_t __iomem *bdp;
        struct sk_buff *skb, *skbn, *skbt;
        int received = 0;
        u16 pkt_len, sc;
        int curidx;
 
-       if (!netif_running(dev))
-               return 0;
-
        /*
         * First, grab all of the stats for the incoming packet.
         * These get messed up if we get called due to a busy condition.
@@ -99,12 +108,10 @@ static int fs_enet_rx_napi(struct napi_struct *napi, int budget)
                 * the last indicator should be set.
                 */
                if ((sc & BD_ENET_RX_LAST) == 0)
-                       printk(KERN_WARNING DRV_MODULE_NAME
-                              ": %s rcv is not +last\n",
-                              dev->name);
+                       dev_warn(fep->dev, "rcv is not +last\n");
 
                /*
-                * Check for errors. 
+                * Check for errors.
                 */
                if (sc & (BD_ENET_RX_LG | BD_ENET_RX_SH | BD_ENET_RX_CL |
                          BD_ENET_RX_NO | BD_ENET_RX_CR | BD_ENET_RX_OV)) {
@@ -156,18 +163,21 @@ static int fs_enet_rx_napi(struct napi_struct *napi, int budget)
                                        skb = skbn;
                                        skbn = skbt;
                                }
-                       } else
+                       } else {
                                skbn = dev_alloc_skb(ENET_RX_FRSIZE);
 
+                               if (skbn)
+                                       skb_align(skbn, ENET_RX_ALIGN);
+                       }
+
                        if (skbn != NULL) {
                                skb_put(skb, pkt_len);  /* Make room */
                                skb->protocol = eth_type_trans(skb, dev);
                                received++;
                                netif_receive_skb(skb);
                        } else {
-                               printk(KERN_WARNING DRV_MODULE_NAME
-                                      ": %s Memory squeeze, dropping packet.\n",
-                                      dev->name);
+                               dev_warn(fep->dev,
+                                        "Memory squeeze, dropping packet.\n");
                                fep->stats.rx_dropped++;
                                skbn = skb;
                        }
@@ -181,7 +191,7 @@ static int fs_enet_rx_napi(struct napi_struct *napi, int budget)
                CBDW_SC(bdp, (sc & ~BD_ENET_RX_STATS) | BD_ENET_RX_EMPTY);
 
                /*
-                * Update BD pointer to next entry. 
+                * Update BD pointer to next entry.
                 */
                if ((sc & BD_ENET_RX_WRAP) == 0)
                        bdp++;
@@ -196,9 +206,9 @@ static int fs_enet_rx_napi(struct napi_struct *napi, int budget)
 
        fep->cur_rx = bdp;
 
-       if (received >= budget) {
+       if (received < budget) {
                /* done */
-               netif_rx_complete(dev, napi);
+               napi_complete(napi);
                (*fep->ops->napi_enable_rx)(dev);
        }
        return received;
@@ -209,7 +219,7 @@ static int fs_enet_rx_non_napi(struct net_device *dev)
 {
        struct fs_enet_private *fep = netdev_priv(dev);
        const struct fs_platform_info *fpi = fep->fpi;
-       cbd_t *bdp;
+       cbd_t __iomem *bdp;
        struct sk_buff *skb, *skbn, *skbt;
        int received = 0;
        u16 pkt_len, sc;
@@ -229,12 +239,10 @@ static int fs_enet_rx_non_napi(struct net_device *dev)
                 * the last indicator should be set.
                 */
                if ((sc & BD_ENET_RX_LAST) == 0)
-                       printk(KERN_WARNING DRV_MODULE_NAME
-                              ": %s rcv is not +last\n",
-                              dev->name);
+                       dev_warn(fep->dev, "rcv is not +last\n");
 
                /*
-                * Check for errors. 
+                * Check for errors.
                 */
                if (sc & (BD_ENET_RX_LG | BD_ENET_RX_SH | BD_ENET_RX_CL |
                          BD_ENET_RX_NO | BD_ENET_RX_CR | BD_ENET_RX_OV)) {
@@ -287,18 +295,21 @@ static int fs_enet_rx_non_napi(struct net_device *dev)
                                        skb = skbn;
                                        skbn = skbt;
                                }
-                       } else
+                       } else {
                                skbn = dev_alloc_skb(ENET_RX_FRSIZE);
 
+                               if (skbn)
+                                       skb_align(skbn, ENET_RX_ALIGN);
+                       }
+
                        if (skbn != NULL) {
                                skb_put(skb, pkt_len);  /* Make room */
                                skb->protocol = eth_type_trans(skb, dev);
                                received++;
                                netif_rx(skb);
                        } else {
-                               printk(KERN_WARNING DRV_MODULE_NAME
-                                      ": %s Memory squeeze, dropping packet.\n",
-                                      dev->name);
+                               dev_warn(fep->dev,
+                                        "Memory squeeze, dropping packet.\n");
                                fep->stats.rx_dropped++;
                                skbn = skb;
                        }
@@ -312,7 +323,7 @@ static int fs_enet_rx_non_napi(struct net_device *dev)
                CBDW_SC(bdp, (sc & ~BD_ENET_RX_STATS) | BD_ENET_RX_EMPTY);
 
                /*
-                * Update BD pointer to next entry. 
+                * Update BD pointer to next entry.
                 */
                if ((sc & BD_ENET_RX_WRAP) == 0)
                        bdp++;
@@ -330,7 +341,7 @@ static int fs_enet_rx_non_napi(struct net_device *dev)
 static void fs_enet_tx(struct net_device *dev)
 {
        struct fs_enet_private *fep = netdev_priv(dev);
-       cbd_t *bdp;
+       cbd_t __iomem *bdp;
        struct sk_buff *skb;
        int dirtyidx, do_wake, do_restart;
        u16 sc;
@@ -340,7 +351,6 @@ static void fs_enet_tx(struct net_device *dev)
 
        do_wake = do_restart = 0;
        while (((sc = CBDR_SC(bdp)) & BD_ENET_TX_READY) == 0) {
-
                dirtyidx = bdp - fep->tx_bd_base;
 
                if (fep->tx_free == fep->tx_ring)
@@ -349,7 +359,7 @@ static void fs_enet_tx(struct net_device *dev)
                skb = fep->tx_skbuff[dirtyidx];
 
                /*
-                * Check for errors. 
+                * Check for errors.
                 */
                if (sc & (BD_ENET_TX_HB | BD_ENET_TX_LC |
                          BD_ENET_TX_RL | BD_ENET_TX_UN | BD_ENET_TX_CSL)) {
@@ -372,10 +382,10 @@ static void fs_enet_tx(struct net_device *dev)
                } else
                        fep->stats.tx_packets++;
 
-               if (sc & BD_ENET_TX_READY)
-                       printk(KERN_WARNING DRV_MODULE_NAME
-                              ": %s HEY! Enet xmit interrupt and TX_READY.\n",
-                              dev->name);
+               if (sc & BD_ENET_TX_READY) {
+                       dev_warn(fep->dev,
+                                "HEY! Enet xmit interrupt and TX_READY.\n");
+               }
 
                /*
                 * Deferred means some collisions occurred during transmit,
@@ -389,13 +399,13 @@ static void fs_enet_tx(struct net_device *dev)
                                skb->len, DMA_TO_DEVICE);
 
                /*
-                * Free the sk buffer associated with this last transmit. 
+                * Free the sk buffer associated with this last transmit.
                 */
                dev_kfree_skb_irq(skb);
                fep->tx_skbuff[dirtyidx] = NULL;
 
                /*
-                * Update pointer to next buffer descriptor to be transmitted. 
+                * Update pointer to next buffer descriptor to be transmitted.
                 */
                if ((sc & BD_ENET_TX_WRAP) == 0)
                        bdp++;
@@ -441,7 +451,6 @@ fs_enet_interrupt(int irq, void *dev_id)
 
        nr = 0;
        while ((int_events = (*fep->ops->get_int_events)(dev)) != 0) {
-
                nr++;
 
                int_clr_events = int_events;
@@ -465,7 +474,7 @@ fs_enet_interrupt(int irq, void *dev_id)
                                /* NOTE: it is possible for FCCs in NAPI mode    */
                                /* to submit a spurious interrupt while in poll  */
                                if (napi_ok)
-                                       __netif_rx_schedule(dev, &fep->napi);
+                                       __napi_schedule(&fep->napi);
                        }
                }
 
@@ -480,7 +489,7 @@ fs_enet_interrupt(int irq, void *dev_id)
 void fs_init_bds(struct net_device *dev)
 {
        struct fs_enet_private *fep = netdev_priv(dev);
-       cbd_t *bdp;
+       cbd_t __iomem *bdp;
        struct sk_buff *skb;
        int i;
 
@@ -491,16 +500,16 @@ void fs_init_bds(struct net_device *dev)
        fep->cur_rx = fep->rx_bd_base;
 
        /*
-        * Initialize the receive buffer descriptors. 
+        * Initialize the receive buffer descriptors.
         */
        for (i = 0, bdp = fep->rx_bd_base; i < fep->rx_ring; i++, bdp++) {
                skb = dev_alloc_skb(ENET_RX_FRSIZE);
                if (skb == NULL) {
-                       printk(KERN_WARNING DRV_MODULE_NAME
-                              ": %s Memory squeeze, unable to allocate skb\n",
-                              dev->name);
+                       dev_warn(fep->dev,
+                                "Memory squeeze, unable to allocate skb\n");
                        break;
                }
+               skb_align(skb, ENET_RX_ALIGN);
                fep->rx_skbuff[i] = skb;
                CBDW_BUFADDR(bdp,
                        dma_map_single(fep->dev, skb->data,
@@ -511,7 +520,7 @@ void fs_init_bds(struct net_device *dev)
                        ((i < fep->rx_ring - 1) ? 0 : BD_SC_WRAP));
        }
        /*
-        * if we failed, fillup remainder 
+        * if we failed, fillup remainder
         */
        for (; i < fep->rx_ring; i++, bdp++) {
                fep->rx_skbuff[i] = NULL;
@@ -519,7 +528,7 @@ void fs_init_bds(struct net_device *dev)
        }
 
        /*
-        * ...and the same for transmit.  
+        * ...and the same for transmit.
         */
        for (i = 0, bdp = fep->tx_bd_base; i < fep->tx_ring; i++, bdp++) {
                fep->tx_skbuff[i] = NULL;
@@ -533,11 +542,11 @@ void fs_cleanup_bds(struct net_device *dev)
 {
        struct fs_enet_private *fep = netdev_priv(dev);
        struct sk_buff *skb;
-       cbd_t *bdp;
+       cbd_t __iomem *bdp;
        int i;
 
        /*
-        * Reset SKB transmit buffers.  
+        * Reset SKB transmit buffers.
         */
        for (i = 0, bdp = fep->tx_bd_base; i < fep->tx_ring; i++, bdp++) {
                if ((skb = fep->tx_skbuff[i]) == NULL)
@@ -552,7 +561,7 @@ void fs_cleanup_bds(struct net_device *dev)
        }
 
        /*
-        * Reset SKB receive buffers 
+        * Reset SKB receive buffers
         */
        for (i = 0, bdp = fep->rx_bd_base; i < fep->rx_ring; i++, bdp++) {
                if ((skb = fep->rx_skbuff[i]) == NULL)
@@ -571,18 +580,65 @@ void fs_cleanup_bds(struct net_device *dev)
 
 /**********************************************************************************/
 
+#ifdef CONFIG_FS_ENET_MPC5121_FEC
+/*
+ * MPC5121 FEC requeries 4-byte alignment for TX data buffer!
+ */
+static struct sk_buff *tx_skb_align_workaround(struct net_device *dev,
+                                              struct sk_buff *skb)
+{
+       struct sk_buff *new_skb;
+       struct fs_enet_private *fep = netdev_priv(dev);
+
+       /* Alloc new skb */
+       new_skb = dev_alloc_skb(skb->len + 4);
+       if (!new_skb) {
+               if (net_ratelimit()) {
+                       dev_warn(fep->dev,
+                                "Memory squeeze, dropping tx packet.\n");
+               }
+               return NULL;
+       }
+
+       /* Make sure new skb is properly aligned */
+       skb_align(new_skb, 4);
+
+       /* Copy data to new skb ... */
+       skb_copy_from_linear_data(skb, new_skb->data, skb->len);
+       skb_put(new_skb, skb->len);
+
+       /* ... and free an old one */
+       dev_kfree_skb_any(skb);
+
+       return new_skb;
+}
+#endif
+
 static int fs_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 {
        struct fs_enet_private *fep = netdev_priv(dev);
-       cbd_t *bdp;
+       cbd_t __iomem *bdp;
        int curidx;
        u16 sc;
        unsigned long flags;
 
+#ifdef CONFIG_FS_ENET_MPC5121_FEC
+       if (((unsigned long)skb->data) & 0x3) {
+               skb = tx_skb_align_workaround(dev, skb);
+               if (!skb) {
+                       /*
+                        * We have lost packet due to memory allocation error
+                        * in tx_skb_align_workaround(). Hopefully original
+                        * skb is still valid, so try transmit it later.
+                        */
+                       return NETDEV_TX_BUSY;
+               }
+       }
+#endif
        spin_lock_irqsave(&fep->tx_lock, flags);
 
        /*
-        * Fill in a Tx ring entry 
+        * Fill in a Tx ring entry
         */
        bdp = fep->cur_tx;
 
@@ -594,35 +650,32 @@ static int fs_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
                 * Ooops.  All transmit buffers are full.  Bail out.
                 * This should not happen, since the tx queue should be stopped.
                 */
-               printk(KERN_WARNING DRV_MODULE_NAME
-                      ": %s tx queue full!.\n", dev->name);
+               dev_warn(fep->dev, "tx queue full!.\n");
                return NETDEV_TX_BUSY;
        }
 
        curidx = bdp - fep->tx_bd_base;
        /*
-        * Clear all of the status flags. 
+        * Clear all of the status flags.
         */
        CBDC_SC(bdp, BD_ENET_TX_STATS);
 
        /*
-        * Save skb pointer. 
+        * Save skb pointer.
         */
        fep->tx_skbuff[curidx] = skb;
 
        fep->stats.tx_bytes += skb->len;
 
        /*
-        * Push the data cache so the CPM does not get stale memory data. 
+        * Push the data cache so the CPM does not get stale memory data.
         */
        CBDW_BUFADDR(bdp, dma_map_single(fep->dev,
                                skb->data, skb->len, DMA_TO_DEVICE));
        CBDW_DATLEN(bdp, skb->len);
 
-       dev->trans_start = jiffies;
-
        /*
-        * If this was the last BD in the ring, start at the beginning again. 
+        * If this was the last BD in the ring, start at the beginning again.
         */
        if ((CBDR_SC(bdp) & BD_ENET_TX_WRAP) == 0)
                fep->cur_tx++;
@@ -650,23 +703,6 @@ static int fs_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
        return NETDEV_TX_OK;
 }
 
-static int fs_request_irq(struct net_device *dev, int irq, const char *name,
-               irq_handler_t irqf)
-{
-       struct fs_enet_private *fep = netdev_priv(dev);
-
-       (*fep->ops->pre_request_irq)(dev, irq);
-       return request_irq(irq, irqf, IRQF_SHARED, name, dev);
-}
-
-static void fs_free_irq(struct net_device *dev, int irq)
-{
-       struct fs_enet_private *fep = netdev_priv(dev);
-
-       free_irq(irq, dev);
-       (*fep->ops->post_free_irq)(dev, irq);
-}
-
 static void fs_timeout(struct net_device *dev)
 {
        struct fs_enet_private *fep = netdev_priv(dev);
@@ -697,45 +733,38 @@ static void fs_timeout(struct net_device *dev)
  *-----------------------------------------------------------------------------*/
 static void generic_adjust_link(struct  net_device *dev)
 {
-       struct fs_enet_private *fep = netdev_priv(dev);
-       struct phy_device *phydev = fep->phydev;
-       int new_state = 0;
-
-       if (phydev->link) {
-
-               /* adjust to duplex mode */
-               if (phydev->duplex != fep->oldduplex){
-                       new_state = 1;
-                       fep->oldduplex = phydev->duplex;
-               }
-
-               if (phydev->speed != fep->oldspeed) {
-                       new_state = 1;
-                       fep->oldspeed = phydev->speed;
-               }
-
-               if (!fep->oldlink) {
-                       new_state = 1;
-                       fep->oldlink = 1;
-                       netif_schedule(dev);
-                       netif_carrier_on(dev);
-                       netif_start_queue(dev);
-               }
-
-               if (new_state)
-                       fep->ops->restart(dev);
-
-       } else if (fep->oldlink) {
-               new_state = 1;
-               fep->oldlink = 0;
-               fep->oldspeed = 0;
-               fep->oldduplex = -1;
-               netif_carrier_off(dev);
-               netif_stop_queue(dev);
-       }
-
-       if (new_state && netif_msg_link(fep))
-               phy_print_status(phydev);
+       struct fs_enet_private *fep = netdev_priv(dev);
+       struct phy_device *phydev = fep->phydev;
+       int new_state = 0;
+
+       if (phydev->link) {
+               /* adjust to duplex mode */
+               if (phydev->duplex != fep->oldduplex) {
+                       new_state = 1;
+                       fep->oldduplex = phydev->duplex;
+               }
+
+               if (phydev->speed != fep->oldspeed) {
+                       new_state = 1;
+                       fep->oldspeed = phydev->speed;
+               }
+
+               if (!fep->oldlink) {
+                       new_state = 1;
+                       fep->oldlink = 1;
+               }
+
+               if (new_state)
+                       fep->ops->restart(dev);
+       } else if (fep->oldlink) {
+               new_state = 1;
+               fep->oldlink = 0;
+               fep->oldspeed = 0;
+               fep->oldduplex = -1;
+       }
+
+       if (new_state && netif_msg_link(fep))
+               phy_print_status(phydev);
 }
 
 
@@ -762,16 +791,16 @@ static int fs_init_phy(struct net_device *dev)
        fep->oldlink = 0;
        fep->oldspeed = 0;
        fep->oldduplex = -1;
-       if(fep->fpi->bus_id)
-               phydev = phy_connect(dev, fep->fpi->bus_id, &fs_adjust_link, 0,
+
+       phydev = of_phy_connect(dev, fep->fpi->phy_node, &fs_adjust_link, 0,
                                PHY_INTERFACE_MODE_MII);
-       else {
-               printk("No phy bus ID specified in BSP code\n");
-               return -EINVAL;
+       if (!phydev) {
+               phydev = of_phy_connect_fixed_link(dev, &fs_adjust_link,
+                                                  PHY_INTERFACE_MODE_MII);
        }
-       if (IS_ERR(phydev)) {
-               printk(KERN_ERR "%s: Could not attach to PHY\n", dev->name);
-               return PTR_ERR(phydev);
+       if (!phydev) {
+               dev_err(&dev->dev, "Could not attach to PHY\n");
+               return -ENODEV;
        }
 
        fep->phydev = phydev;
@@ -779,31 +808,40 @@ static int fs_init_phy(struct net_device *dev)
        return 0;
 }
 
-
 static int fs_enet_open(struct net_device *dev)
 {
        struct fs_enet_private *fep = netdev_priv(dev);
        int r;
        int err;
 
-       napi_enable(&fep->napi);
+       /* to initialize the fep->cur_rx,... */
+       /* not doing this, will cause a crash in fs_enet_rx_napi */
+       fs_init_bds(fep->ndev);
+
+       if (fep->fpi->use_napi)
+               napi_enable(&fep->napi);
 
        /* Install our interrupt handler. */
-       r = fs_request_irq(dev, fep->interrupt, "fs_enet-mac", fs_enet_interrupt);
+       r = request_irq(fep->interrupt, fs_enet_interrupt, IRQF_SHARED,
+                       "fs_enet-mac", dev);
        if (r != 0) {
-               printk(KERN_ERR DRV_MODULE_NAME
-                      ": %s Could not allocate FS_ENET IRQ!", dev->name);
-               napi_disable(&fep->napi);
+               dev_err(fep->dev, "Could not allocate FS_ENET IRQ!");
+               if (fep->fpi->use_napi)
+                       napi_disable(&fep->napi);
                return -EINVAL;
        }
 
        err = fs_init_phy(dev);
-       if(err) {
-               napi_disable(&fep->napi);
+       if (err) {
+               free_irq(fep->interrupt, dev);
+               if (fep->fpi->use_napi)
+                       napi_disable(&fep->napi);
                return err;
        }
        phy_start(fep->phydev);
 
+       netif_start_queue(dev);
+
        return 0;
 }
 
@@ -814,7 +852,8 @@ static int fs_enet_close(struct net_device *dev)
 
        netif_stop_queue(dev);
        netif_carrier_off(dev);
-       napi_disable(&fep->napi);
+       if (fep->fpi->use_napi)
+               napi_disable(&fep->napi);
        phy_stop(fep->phydev);
 
        spin_lock_irqsave(&fep->lock, flags);
@@ -826,7 +865,7 @@ static int fs_enet_close(struct net_device *dev)
        /* release any irqs */
        phy_disconnect(fep->phydev);
        fep->phydev = NULL;
-       fs_free_irq(dev, fep->interrupt);
+       free_irq(fep->interrupt, dev);
 
        return 0;
 }
@@ -873,14 +912,21 @@ static void fs_get_regs(struct net_device *dev, struct ethtool_regs *regs,
 static int fs_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 {
        struct fs_enet_private *fep = netdev_priv(dev);
+
+       if (!fep->phydev)
+               return -ENODEV;
+
        return phy_ethtool_gset(fep->phydev, cmd);
 }
 
 static int fs_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 {
        struct fs_enet_private *fep = netdev_priv(dev);
-       phy_ethtool_sset(fep->phydev, cmd);
-       return 0;
+
+       if (!fep->phydev)
+               return -ENODEV;
+
+       return phy_ethtool_sset(fep->phydev, cmd);
 }
 
 static int fs_nway_reset(struct net_device *dev)
@@ -917,350 +963,226 @@ static const struct ethtool_ops fs_ethtool_ops = {
 static int fs_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 {
        struct fs_enet_private *fep = netdev_priv(dev);
-       struct mii_ioctl_data *mii = (struct mii_ioctl_data *)&rq->ifr_data;
-       unsigned long flags;
-       int rc;
 
        if (!netif_running(dev))
                return -EINVAL;
 
-       spin_lock_irqsave(&fep->lock, flags);
-       rc = phy_mii_ioctl(fep->phydev, mii, cmd);
-       spin_unlock_irqrestore(&fep->lock, flags);
-       return rc;
+       return phy_mii_ioctl(fep->phydev, rq, cmd);
 }
 
 extern int fs_mii_connect(struct net_device *dev);
 extern void fs_mii_disconnect(struct net_device *dev);
 
-static struct net_device *fs_init_instance(struct device *dev,
-               struct fs_platform_info *fpi)
-{
-       struct net_device *ndev = NULL;
-       struct fs_enet_private *fep = NULL;
-       int privsize, i, r, err = 0, registered = 0;
+/**************************************************************************************/
 
-       fpi->fs_no = fs_get_id(fpi);
-       /* guard */
-       if ((unsigned int)fpi->fs_no >= FS_MAX_INDEX)
-               return ERR_PTR(-EINVAL);
+#ifdef CONFIG_FS_ENET_HAS_FEC
+#define IS_FEC(match) ((match)->data == &fs_fec_ops)
+#else
+#define IS_FEC(match) 0
+#endif
 
-       privsize = sizeof(*fep) + (sizeof(struct sk_buff **) *
-                           (fpi->rx_ring + fpi->tx_ring));
+static const struct net_device_ops fs_enet_netdev_ops = {
+       .ndo_open               = fs_enet_open,
+       .ndo_stop               = fs_enet_close,
+       .ndo_get_stats          = fs_enet_get_stats,
+       .ndo_start_xmit         = fs_enet_start_xmit,
+       .ndo_tx_timeout         = fs_timeout,
+       .ndo_set_multicast_list = fs_set_multicast_list,
+       .ndo_do_ioctl           = fs_ioctl,
+       .ndo_validate_addr      = eth_validate_addr,
+       .ndo_set_mac_address    = eth_mac_addr,
+       .ndo_change_mtu         = eth_change_mtu,
+#ifdef CONFIG_NET_POLL_CONTROLLER
+       .ndo_poll_controller    = fs_enet_netpoll,
+#endif
+};
 
-       ndev = alloc_etherdev(privsize);
-       if (!ndev) {
-               err = -ENOMEM;
-               goto err;
-       }
+static int __devinit fs_enet_probe(struct platform_device *ofdev,
+                                   const struct of_device_id *match)
+{
+       struct net_device *ndev;
+       struct fs_enet_private *fep;
+       struct fs_platform_info *fpi;
+       const u32 *data;
+       const u8 *mac_addr;
+       int privsize, len, ret = -ENODEV;
 
-       fep = netdev_priv(ndev);
+       fpi = kzalloc(sizeof(*fpi), GFP_KERNEL);
+       if (!fpi)
+               return -ENOMEM;
 
-       fep->dev = dev;
-       dev_set_drvdata(dev, ndev);
-       fep->fpi = fpi;
-       if (fpi->init_ioports)
-               fpi->init_ioports((struct fs_platform_info *)fpi);
+       if (!IS_FEC(match)) {
+               data = of_get_property(ofdev->dev.of_node, "fsl,cpm-command", &len);
+               if (!data || len != 4)
+                       goto out_free_fpi;
 
-#ifdef CONFIG_FS_ENET_HAS_FEC
-       if (fs_get_fec_index(fpi->fs_no) >= 0)
-               fep->ops = &fs_fec_ops;
-#endif
+               fpi->cp_command = *data;
+       }
 
-#ifdef CONFIG_FS_ENET_HAS_SCC
-       if (fs_get_scc_index(fpi->fs_no) >=0 )
-               fep->ops = &fs_scc_ops;
-#endif
+       fpi->rx_ring = 32;
+       fpi->tx_ring = 32;
+       fpi->rx_copybreak = 240;
+       fpi->use_napi = 1;
+       fpi->napi_weight = 17;
+       fpi->phy_node = of_parse_phandle(ofdev->dev.of_node, "phy-handle", 0);
+       if ((!fpi->phy_node) && (!of_get_property(ofdev->dev.of_node, "fixed-link",
+                                                 NULL)))
+               goto out_free_fpi;
 
-#ifdef CONFIG_FS_ENET_HAS_FCC
-       if (fs_get_fcc_index(fpi->fs_no) >= 0)
-               fep->ops = &fs_fcc_ops;
-#endif
+       privsize = sizeof(*fep) +
+                  sizeof(struct sk_buff **) *
+                  (fpi->rx_ring + fpi->tx_ring);
 
-       if (fep->ops == NULL) {
-               printk(KERN_ERR DRV_MODULE_NAME
-                      ": %s No matching ops found (%d).\n",
-                      ndev->name, fpi->fs_no);
-               err = -EINVAL;
-               goto err;
+       ndev = alloc_etherdev(privsize);
+       if (!ndev) {
+               ret = -ENOMEM;
+               goto out_put;
        }
 
-       r = (*fep->ops->setup_data)(ndev);
-       if (r != 0) {
-               printk(KERN_ERR DRV_MODULE_NAME
-                      ": %s setup_data failed\n",
-                       ndev->name);
-               err = r;
-               goto err;
-       }
+       SET_NETDEV_DEV(ndev, &ofdev->dev);
+       dev_set_drvdata(&ofdev->dev, ndev);
+
+       fep = netdev_priv(ndev);
+       fep->dev = &ofdev->dev;
+       fep->ndev = ndev;
+       fep->fpi = fpi;
+       fep->ops = match->data;
+
+       ret = fep->ops->setup_data(ndev);
+       if (ret)
+               goto out_free_dev;
 
-       /* point rx_skbuff, tx_skbuff */
        fep->rx_skbuff = (struct sk_buff **)&fep[1];
        fep->tx_skbuff = fep->rx_skbuff + fpi->rx_ring;
 
-       /* init locks */
        spin_lock_init(&fep->lock);
        spin_lock_init(&fep->tx_lock);
 
-       /*
-        * Set the Ethernet address. 
-        */
-       for (i = 0; i < 6; i++)
-               ndev->dev_addr[i] = fpi->macaddr[i];
-       
-       r = (*fep->ops->allocate_bd)(ndev);
-       
-       if (fep->ring_base == NULL) {
-               printk(KERN_ERR DRV_MODULE_NAME
-                      ": %s buffer descriptor alloc failed (%d).\n", ndev->name, r);
-               err = r;
-               goto err;
-       }
+       mac_addr = of_get_mac_address(ofdev->dev.of_node);
+       if (mac_addr)
+               memcpy(ndev->dev_addr, mac_addr, 6);
+
+       ret = fep->ops->allocate_bd(ndev);
+       if (ret)
+               goto out_cleanup_data;
 
-       /*
-        * Set receive and transmit descriptor base.
-        */
        fep->rx_bd_base = fep->ring_base;
        fep->tx_bd_base = fep->rx_bd_base + fpi->rx_ring;
 
-       /* initialize ring size variables */
        fep->tx_ring = fpi->tx_ring;
        fep->rx_ring = fpi->rx_ring;
 
-       /*
-        * The FEC Ethernet specific entries in the device structure. 
-        */
-       ndev->open = fs_enet_open;
-       ndev->hard_start_xmit = fs_enet_start_xmit;
-       ndev->tx_timeout = fs_timeout;
+       ndev->netdev_ops = &fs_enet_netdev_ops;
        ndev->watchdog_timeo = 2 * HZ;
-       ndev->stop = fs_enet_close;
-       ndev->get_stats = fs_enet_get_stats;
-       ndev->set_multicast_list = fs_set_multicast_list;
-       netif_napi_add(ndev, &fep->napi,
-                      fs_enet_rx_napi, fpi->napi_weight);
+       if (fpi->use_napi)
+               netif_napi_add(ndev, &fep->napi, fs_enet_rx_napi,
+                              fpi->napi_weight);
 
        ndev->ethtool_ops = &fs_ethtool_ops;
-       ndev->do_ioctl = fs_ioctl;
 
        init_timer(&fep->phy_timer_list);
 
        netif_carrier_off(ndev);
 
-       err = register_netdev(ndev);
-       if (err != 0) {
-               printk(KERN_ERR DRV_MODULE_NAME
-                      ": %s register_netdev failed.\n", ndev->name);
-               goto err;
-       }
-       registered = 1;
-
-
-       return ndev;
-
-      err:
-       if (ndev != NULL) {
+       ret = register_netdev(ndev);
+       if (ret)
+               goto out_free_bd;
 
-               if (registered)
-                       unregister_netdev(ndev);
+       pr_info("%s: fs_enet: %pM\n", ndev->name, ndev->dev_addr);
 
-               if (fep != NULL) {
-                       (*fep->ops->free_bd)(ndev);
-                       (*fep->ops->cleanup_data)(ndev);
-               }
-
-               free_netdev(ndev);
-       }
-
-       dev_set_drvdata(dev, NULL);
+       return 0;
 
-       return ERR_PTR(err);
+out_free_bd:
+       fep->ops->free_bd(ndev);
+out_cleanup_data:
+       fep->ops->cleanup_data(ndev);
+out_free_dev:
+       free_netdev(ndev);
+       dev_set_drvdata(&ofdev->dev, NULL);
+out_put:
+       of_node_put(fpi->phy_node);
+out_free_fpi:
+       kfree(fpi);
+       return ret;
 }
 
-static int fs_cleanup_instance(struct net_device *ndev)
+static int fs_enet_remove(struct platform_device *ofdev)
 {
-       struct fs_enet_private *fep;
-       const struct fs_platform_info *fpi;
-       struct device *dev;
-
-       if (ndev == NULL)
-               return -EINVAL;
-
-       fep = netdev_priv(ndev);
-       if (fep == NULL)
-               return -EINVAL;
-
-       fpi = fep->fpi;
+       struct net_device *ndev = dev_get_drvdata(&ofdev->dev);
+       struct fs_enet_private *fep = netdev_priv(ndev);
 
        unregister_netdev(ndev);
 
-       dma_free_coherent(fep->dev, (fpi->tx_ring + fpi->rx_ring) * sizeof(cbd_t),
-                         fep->ring_base, fep->ring_mem_addr);
-
-       /* reset it */
-       (*fep->ops->cleanup_data)(ndev);
-
-       dev = fep->dev;
-       if (dev != NULL) {
-               dev_set_drvdata(dev, NULL);
-               fep->dev = NULL;
-       }
-
+       fep->ops->free_bd(ndev);
+       fep->ops->cleanup_data(ndev);
+       dev_set_drvdata(fep->dev, NULL);
+       of_node_put(fep->fpi->phy_node);
        free_netdev(ndev);
-
        return 0;
 }
 
-/**************************************************************************************/
-
-/* handy pointer to the immap */
-void *fs_enet_immap = NULL;
-
-static int setup_immap(void)
-{
-       phys_addr_t paddr = 0;
-       unsigned long size = 0;
-
-#ifdef CONFIG_CPM1
-       paddr = IMAP_ADDR;
-       size = 0x10000; /* map 64K */
+static struct of_device_id fs_enet_match[] = {
+#ifdef CONFIG_FS_ENET_HAS_SCC
+       {
+               .compatible = "fsl,cpm1-scc-enet",
+               .data = (void *)&fs_scc_ops,
+       },
+       {
+               .compatible = "fsl,cpm2-scc-enet",
+               .data = (void *)&fs_scc_ops,
+       },
 #endif
-
-#ifdef CONFIG_CPM2
-       paddr = CPM_MAP_ADDR;
-       size = 0x40000; /* map 256 K */
+#ifdef CONFIG_FS_ENET_HAS_FCC
+       {
+               .compatible = "fsl,cpm2-fcc-enet",
+               .data = (void *)&fs_fcc_ops,
+       },
 #endif
-       fs_enet_immap = ioremap(paddr, size);
-       if (fs_enet_immap == NULL)
-               return -EBADF;  /* XXX ahem; maybe just BUG_ON? */
-
-       return 0;
-}
-
-static void cleanup_immap(void)
-{
-       if (fs_enet_immap != NULL) {
-               iounmap(fs_enet_immap);
-               fs_enet_immap = NULL;
-       }
-}
-
-/**************************************************************************************/
-
-static int __devinit fs_enet_probe(struct device *dev)
-{
-       struct net_device *ndev;
-
-       /* no fixup - no device */
-       if (dev->platform_data == NULL) {
-               printk(KERN_INFO "fs_enet: "
-                               "probe called with no platform data; "
-                               "remove unused devices\n");
-               return -ENODEV;
-       }
-
-       ndev = fs_init_instance(dev, dev->platform_data);
-       if (IS_ERR(ndev))
-               return PTR_ERR(ndev);
-       return 0;
-}
-
-static int fs_enet_remove(struct device *dev)
-{
-       return fs_cleanup_instance(dev_get_drvdata(dev));
-}
-
-static struct device_driver fs_enet_fec_driver = {
-       .name           = "fsl-cpm-fec",
-       .bus            = &platform_bus_type,
-       .probe          = fs_enet_probe,
-       .remove         = fs_enet_remove,
-#ifdef CONFIG_PM
-/*     .suspend        = fs_enet_suspend,      TODO */
-/*     .resume         = fs_enet_resume,       TODO */
+#ifdef CONFIG_FS_ENET_HAS_FEC
+#ifdef CONFIG_FS_ENET_MPC5121_FEC
+       {
+               .compatible = "fsl,mpc5121-fec",
+               .data = (void *)&fs_fec_ops,
+       },
+#else
+       {
+               .compatible = "fsl,pq1-fec-enet",
+               .data = (void *)&fs_fec_ops,
+       },
 #endif
-};
-
-static struct device_driver fs_enet_scc_driver = {
-       .name           = "fsl-cpm-scc",
-       .bus            = &platform_bus_type,
-       .probe          = fs_enet_probe,
-       .remove         = fs_enet_remove,
-#ifdef CONFIG_PM
-/*     .suspend        = fs_enet_suspend,      TODO */
-/*     .resume         = fs_enet_resume,       TODO */
 #endif
+       {}
 };
-
-static struct device_driver fs_enet_fcc_driver = {
-       .name           = "fsl-cpm-fcc",
-       .bus            = &platform_bus_type,
-       .probe          = fs_enet_probe,
-       .remove         = fs_enet_remove,
-#ifdef CONFIG_PM
-/*     .suspend        = fs_enet_suspend,      TODO */
-/*     .resume         = fs_enet_resume,       TODO */
-#endif
+MODULE_DEVICE_TABLE(of, fs_enet_match);
+
+static struct of_platform_driver fs_enet_driver = {
+       .driver = {
+               .owner = THIS_MODULE,
+               .name = "fs_enet",
+               .of_match_table = fs_enet_match,
+       },
+       .probe = fs_enet_probe,
+       .remove = fs_enet_remove,
 };
 
 static int __init fs_init(void)
 {
-       int r;
-
-       printk(KERN_INFO
-                       "%s", version);
-
-       r = setup_immap();
-       if (r != 0)
-               return r;
-
-#ifdef CONFIG_FS_ENET_HAS_FCC
-       /* let's insert mii stuff */
-       r = fs_enet_mdio_bb_init();
-
-       if (r != 0) {
-               printk(KERN_ERR DRV_MODULE_NAME
-                       "BB PHY init failed.\n");
-               return r;
-       }
-       r = driver_register(&fs_enet_fcc_driver);
-       if (r != 0)
-               goto err;
-#endif
-
-#ifdef CONFIG_FS_ENET_HAS_FEC
-       r =  fs_enet_mdio_fec_init();
-       if (r != 0) {
-               printk(KERN_ERR DRV_MODULE_NAME
-                       "FEC PHY init failed.\n");
-               return r;
-       }
-
-       r = driver_register(&fs_enet_fec_driver);
-       if (r != 0)
-               goto err;
-#endif
-
-#ifdef CONFIG_FS_ENET_HAS_SCC
-       r = driver_register(&fs_enet_scc_driver);
-       if (r != 0)
-               goto err;
-#endif
-
-       return 0;
-err:
-       cleanup_immap();
-       return r;
-       
+       return of_register_platform_driver(&fs_enet_driver);
 }
 
 static void __exit fs_cleanup(void)
 {
-       driver_unregister(&fs_enet_fec_driver);
-       driver_unregister(&fs_enet_fcc_driver);
-       driver_unregister(&fs_enet_scc_driver);
-       cleanup_immap();
+       of_unregister_platform_driver(&fs_enet_driver);
+}
+
+#ifdef CONFIG_NET_POLL_CONTROLLER
+static void fs_enet_netpoll(struct net_device *dev)
+{
+       disable_irq(dev->irq);
+       fs_enet_interrupt(dev->irq, dev);
+       enable_irq(dev->irq);
 }
+#endif
 
 /**************************************************************************************/