1 files changed, 64 insertions, 47 deletions

diff --git a/drivers/net/r6040.c b/drivers/net/r6040.c
index 27e6f6d43cac..7965ae42eae4 100644
--- a/drivers/net/r6040.c
+++ b/drivers/net/r6040.c
@@ -69,6 +69,8 @@
 
 /* MAC registers */
 #define MCR0            0x00    /* Control register 0 */
+#define  MCR0_PROMISC   0x0020  /* Promiscuous mode */
+#define  MCR0_HASH_EN   0x0100  /* Enable multicast hash table function */
 #define MCR1            0x04    /* Control register 1 */
 #define  MAC_RST        0x0001  /* Reset the MAC */
 #define MBCR            0x08    /* Bus control */
@@ -851,77 +853,92 @@ static void r6040_multicast_list(struct net_device *dev)
 {
         struct r6040_private *lp = netdev_priv(dev);
         void __iomem *ioaddr = lp->base;
-        u16 *adrp;
-        u16 reg;
         unsigned long flags;
         struct netdev_hw_addr *ha;
         int i;
+        u16 *adrp;
+        u16 hash_table[4] = { 0 };
+
+        spin_lock_irqsave(&lp->lock, flags);
 
-        /* MAC Address */
+        /* Keep our MAC Address */
         adrp = (u16 *)dev->dev_addr;
         iowrite16(adrp[0], ioaddr + MID_0L);
         iowrite16(adrp[1], ioaddr + MID_0M);
         iowrite16(adrp[2], ioaddr + MID_0H);
 
-        /* Promiscous Mode */
-        spin_lock_irqsave(&lp->lock, flags);
-
         /* Clear AMCP & PROM bits */
-        reg = ioread16(ioaddr) & ~0x0120;
-        if (dev->flags & IFF_PROMISC) {
-                reg |= 0x0020;
-                lp->mcr0 |= 0x0020;
-        }
-        /* Too many multicast addresses
-         * accept all traffic */
-        else if ((netdev_mc_count(dev) > MCAST_MAX) ||
-                 (dev->flags & IFF_ALLMULTI))
-                reg |= 0x0020;
+        lp->mcr0 = ioread16(ioaddr + MCR0) & ~(MCR0_PROMISC | MCR0_HASH_EN);
 
-        iowrite16(reg, ioaddr);
-        spin_unlock_irqrestore(&lp->lock, flags);
+        /* Promiscuous mode */
+        if (dev->flags & IFF_PROMISC)
+                lp->mcr0 |= MCR0_PROMISC;
 
-        /* Build the hash table */
-        if (netdev_mc_count(dev) > MCAST_MAX) {
-                u16 hash_table[4];
-                u32 crc;
+        /* Enable multicast hash table function to
+         * receive all multicast packets. */
+        else if (dev->flags & IFF_ALLMULTI) {
+                lp->mcr0 |= MCR0_HASH_EN;
 
-                for (i = 0; i < 4; i++)
-                        hash_table[i] = 0;
+                for (i = 0; i < MCAST_MAX ; i++) {
+                        iowrite16(0, ioaddr + MID_1L + 8 * i);
+                        iowrite16(0, ioaddr + MID_1M + 8 * i);
+                        iowrite16(0, ioaddr + MID_1H + 8 * i);
+                }
 
+                for (i = 0; i < 4; i++)
+                        hash_table[i] = 0xffff;
+        }
+        /* Use internal multicast address registers if the number of
+         * multicast addresses is not greater than MCAST_MAX. */
+        else if (netdev_mc_count(dev) <= MCAST_MAX) {
+                i = 0;
                 netdev_for_each_mc_addr(ha, dev) {
-                        char *addrs = ha->addr;
+                        u16 *adrp = (u16 *) ha->addr;
+                        iowrite16(adrp[0], ioaddr + MID_1L + 8 * i);
+                        iowrite16(adrp[1], ioaddr + MID_1M + 8 * i);
+                        iowrite16(adrp[2], ioaddr + MID_1H + 8 * i);
+                        i++;
+                }
+                while (i < MCAST_MAX) {
+                        iowrite16(0, ioaddr + MID_1L + 8 * i);
+                        iowrite16(0, ioaddr + MID_1M + 8 * i);
+                        iowrite16(0, ioaddr + MID_1H + 8 * i);
+                        i++;
+                }
+        }
+        /* Otherwise, Enable multicast hash table function. */
+        else {
+                u32 crc;
 
-                        if (!(*addrs & 1))
-                                continue;
+                lp->mcr0 |= MCR0_HASH_EN;
+
+                for (i = 0; i < MCAST_MAX ; i++) {
+                        iowrite16(0, ioaddr + MID_1L + 8 * i);
+                        iowrite16(0, ioaddr + MID_1M + 8 * i);
+                        iowrite16(0, ioaddr + MID_1H + 8 * i);
+                }
 
-                        crc = ether_crc_le(6, addrs);
+                /* Build multicast hash table */
+                netdev_for_each_mc_addr(ha, dev) {
+                        u8 *addrs = ha->addr;
+
+                        crc = ether_crc(ETH_ALEN, addrs);
                         crc >>= 26;
-                        hash_table[crc >> 4] |= 1 << (15 - (crc & 0xf));
+                        hash_table[crc >> 4] |= 1 << (crc & 0xf);
                 }
-                /* Fill the MAC hash tables with their values */
+        }
+
+        iowrite16(lp->mcr0, ioaddr + MCR0);
+
+        /* Fill the MAC hash tables with their values */
+        if (lp->mcr0 && MCR0_HASH_EN) {
                 iowrite16(hash_table[0], ioaddr + MAR0);
                 iowrite16(hash_table[1], ioaddr + MAR1);
                 iowrite16(hash_table[2], ioaddr + MAR2);
                 iowrite16(hash_table[3], ioaddr + MAR3);
         }
-        /* Multicast Address 1~4 case */
-        i = 0;
-        netdev_for_each_mc_addr(ha, dev) {
-                if (i >= MCAST_MAX)
-                        break;
-                adrp = (u16 *) ha->addr;
-                iowrite16(adrp[0], ioaddr + MID_1L + 8 * i);
-                iowrite16(adrp[1], ioaddr + MID_1M + 8 * i);
-                iowrite16(adrp[2], ioaddr + MID_1H + 8 * i);
-                i++;
-        }
-        while (i < MCAST_MAX) {
-                iowrite16(0xffff, ioaddr + MID_1L + 8 * i);
-                iowrite16(0xffff, ioaddr + MID_1M + 8 * i);
-                iowrite16(0xffff, ioaddr + MID_1H + 8 * i);
-                i++;
-        }
+
+        spin_unlock_irqrestore(&lp->lock, flags);
 }
 
 static void netdev_get_drvinfo(struct net_device *dev,