mv643xx_eth: implement ->set_rx_mode()
[linux-2.6.git] / drivers / net / mv643xx_eth.c
index 3326587..9f3ee7d 100644 (file)
@@ -1448,11 +1448,8 @@ static const struct ethtool_ops mv643xx_eth_ethtool_ops_phyless = {
 /* address handling *********************************************************/
 static void uc_addr_get(struct mv643xx_eth_private *mp, unsigned char *addr)
 {
-       unsigned int mac_h;
-       unsigned int mac_l;
-
-       mac_h = rdlp(mp, MAC_ADDR_HIGH);
-       mac_l = rdlp(mp, MAC_ADDR_LOW);
+       unsigned int mac_h = rdlp(mp, MAC_ADDR_HIGH);
+       unsigned int mac_l = rdlp(mp, MAC_ADDR_LOW);
 
        addr[0] = (mac_h >> 24) & 0xff;
        addr[1] = (mac_h >> 16) & 0xff;
@@ -1462,57 +1459,71 @@ static void uc_addr_get(struct mv643xx_eth_private *mp, unsigned char *addr)
        addr[5] = mac_l & 0xff;
 }
 
-static void init_mac_tables(struct mv643xx_eth_private *mp)
+static void uc_addr_set(struct mv643xx_eth_private *mp, unsigned char *addr)
 {
-       int i;
-
-       for (i = 0; i < 0x100; i += 4) {
-               wrl(mp, SPECIAL_MCAST_TABLE(mp->port_num) + i, 0);
-               wrl(mp, OTHER_MCAST_TABLE(mp->port_num) + i, 0);
-       }
-
-       for (i = 0; i < 0x10; i += 4)
-               wrl(mp, UNICAST_TABLE(mp->port_num) + i, 0);
+       wrlp(mp, MAC_ADDR_HIGH,
+               (addr[0] << 24) | (addr[1] << 16) | (addr[2] << 8) | addr[3]);
+       wrlp(mp, MAC_ADDR_LOW, (addr[4] << 8) | addr[5]);
 }
 
-static void set_filter_table_entry(struct mv643xx_eth_private *mp,
-                                  int table, unsigned char entry)
+static u32 uc_addr_filter_mask(struct net_device *dev)
 {
-       unsigned int table_reg;
-
-       /* Set "accepts frame bit" at specified table entry */
-       table_reg = rdl(mp, table + (entry & 0xfc));
-       table_reg |= 0x01 << (8 * (entry & 3));
-       wrl(mp, table + (entry & 0xfc), table_reg);
-}
+       struct dev_addr_list *uc_ptr;
+       u32 nibbles;
 
-static void uc_addr_set(struct mv643xx_eth_private *mp, unsigned char *addr)
-{
-       unsigned int mac_h;
-       unsigned int mac_l;
-       int table;
+       if (dev->flags & IFF_PROMISC)
+               return 0;
 
-       mac_l = (addr[4] << 8) | addr[5];
-       mac_h = (addr[0] << 24) | (addr[1] << 16) | (addr[2] << 8) | addr[3];
+       nibbles = 1 << (dev->dev_addr[5] & 0x0f);
+       for (uc_ptr = dev->uc_list; uc_ptr != NULL; uc_ptr = uc_ptr->next) {
+               if (memcmp(dev->dev_addr, uc_ptr->da_addr, 5))
+                       return 0;
+               if ((dev->dev_addr[5] ^ uc_ptr->da_addr[5]) & 0xf0)
+                       return 0;
 
-       wrlp(mp, MAC_ADDR_LOW, mac_l);
-       wrlp(mp, MAC_ADDR_HIGH, mac_h);
+               nibbles |= 1 << (uc_ptr->da_addr[5] & 0x0f);
+       }
 
-       table = UNICAST_TABLE(mp->port_num);
-       set_filter_table_entry(mp, table, addr[5] & 0x0f);
+       return nibbles;
 }
 
-static int mv643xx_eth_set_mac_address(struct net_device *dev, void *addr)
+static void mv643xx_eth_program_unicast_filter(struct net_device *dev)
 {
        struct mv643xx_eth_private *mp = netdev_priv(dev);
+       u32 port_config;
+       u32 nibbles;
+       int i;
 
-       /* +2 is for the offset of the HW addr type */
-       memcpy(dev->dev_addr, addr + 2, 6);
-
-       init_mac_tables(mp);
        uc_addr_set(mp, dev->dev_addr);
 
-       return 0;
+       port_config = rdlp(mp, PORT_CONFIG);
+       nibbles = uc_addr_filter_mask(dev);
+       if (!nibbles) {
+               port_config |= UNICAST_PROMISCUOUS_MODE;
+               wrlp(mp, PORT_CONFIG, port_config);
+               return;
+       }
+
+       for (i = 0; i < 16; i += 4) {
+               int off = UNICAST_TABLE(mp->port_num) + i;
+               u32 v;
+
+               v = 0;
+               if (nibbles & 1)
+                       v |= 0x00000001;
+               if (nibbles & 2)
+                       v |= 0x00000100;
+               if (nibbles & 4)
+                       v |= 0x00010000;
+               if (nibbles & 8)
+                       v |= 0x01000000;
+               nibbles >>= 4;
+
+               wrl(mp, off, v);
+       }
+
+       port_config &= ~UNICAST_PROMISCUOUS_MODE;
+       wrlp(mp, PORT_CONFIG, port_config);
 }
 
 static int addr_crc(unsigned char *addr)
@@ -1533,24 +1544,22 @@ static int addr_crc(unsigned char *addr)
        return crc;
 }
 
-static void mv643xx_eth_set_rx_mode(struct net_device *dev)
+static void mv643xx_eth_program_multicast_filter(struct net_device *dev)
 {
        struct mv643xx_eth_private *mp = netdev_priv(dev);
-       u32 port_config;
+       u32 *mc_spec;
+       u32 *mc_other;
        struct dev_addr_list *addr;
        int i;
 
-       port_config = rdlp(mp, PORT_CONFIG);
-       if (dev->flags & IFF_PROMISC)
-               port_config |= UNICAST_PROMISCUOUS_MODE;
-       else
-               port_config &= ~UNICAST_PROMISCUOUS_MODE;
-       wrlp(mp, PORT_CONFIG, port_config);
-
        if (dev->flags & (IFF_PROMISC | IFF_ALLMULTI)) {
-               int port_num = mp->port_num;
-               u32 accept = 0x01010101;
+               int port_num;
+               u32 accept;
+               int i;
 
+oom:
+               port_num = mp->port_num;
+               accept = 0x01010101;
                for (i = 0; i < 0x100; i += 4) {
                        wrl(mp, SPECIAL_MCAST_TABLE(port_num) + i, accept);
                        wrl(mp, OTHER_MCAST_TABLE(port_num) + i, accept);
@@ -1558,28 +1567,55 @@ static void mv643xx_eth_set_rx_mode(struct net_device *dev)
                return;
        }
 
-       for (i = 0; i < 0x100; i += 4) {
-               wrl(mp, SPECIAL_MCAST_TABLE(mp->port_num) + i, 0);
-               wrl(mp, OTHER_MCAST_TABLE(mp->port_num) + i, 0);
-       }
+       mc_spec = kmalloc(0x200, GFP_KERNEL);
+       if (mc_spec == NULL)
+               goto oom;
+       mc_other = mc_spec + (0x100 >> 2);
+
+       memset(mc_spec, 0, 0x100);
+       memset(mc_other, 0, 0x100);
 
        for (addr = dev->mc_list; addr != NULL; addr = addr->next) {
                u8 *a = addr->da_addr;
-               int table;
-
-               if (addr->da_addrlen != 6)
-                       continue;
+               u32 *table;
+               int entry;
 
                if (memcmp(a, "\x01\x00\x5e\x00\x00", 5) == 0) {
-                       table = SPECIAL_MCAST_TABLE(mp->port_num);
-                       set_filter_table_entry(mp, table, a[5]);
+                       table = mc_spec;
+                       entry = a[5];
                } else {
-                       int crc = addr_crc(a);
-
-                       table = OTHER_MCAST_TABLE(mp->port_num);
-                       set_filter_table_entry(mp, table, crc);
+                       table = mc_other;
+                       entry = addr_crc(a);
                }
+
+               table[entry >> 2] |= 1 << (entry & 3);
        }
+
+       for (i = 0; i < 0x100; i += 4) {
+               wrl(mp, SPECIAL_MCAST_TABLE(mp->port_num) + i, mc_spec[i >> 2]);
+               wrl(mp, OTHER_MCAST_TABLE(mp->port_num) + i, mc_other[i >> 2]);
+       }
+
+       kfree(mc_spec);
+}
+
+static void mv643xx_eth_set_rx_mode(struct net_device *dev)
+{
+       mv643xx_eth_program_unicast_filter(dev);
+       mv643xx_eth_program_multicast_filter(dev);
+}
+
+static int mv643xx_eth_set_mac_address(struct net_device *dev, void *addr)
+{
+       struct sockaddr *sa = addr;
+
+       memcpy(dev->dev_addr, sa->sa_data, ETH_ALEN);
+
+       netif_addr_lock_bh(dev);
+       mv643xx_eth_program_unicast_filter(dev);
+       netif_addr_unlock_bh(dev);
+
+       return 0;
 }
 
 
@@ -1988,7 +2024,7 @@ static void port_start(struct mv643xx_eth_private *mp)
        /*
         * Add configured unicast address to address filter table.
         */
-       uc_addr_set(mp, mp->dev->dev_addr);
+       mv643xx_eth_program_unicast_filter(mp->dev);
 
        /*
         * Receive all unmatched unicast, TCP, UDP, BPDU and broadcast
@@ -2084,8 +2120,6 @@ static int mv643xx_eth_open(struct net_device *dev)
                return -EAGAIN;
        }
 
-       init_mac_tables(mp);
-
        mv643xx_eth_recalc_skb_size(mp);
 
        napi_enable(&mp->napi);
@@ -2667,7 +2701,7 @@ static int mv643xx_eth_probe(struct platform_device *pdev)
        dev->hard_start_xmit = mv643xx_eth_xmit;
        dev->open = mv643xx_eth_open;
        dev->stop = mv643xx_eth_stop;
-       dev->set_multicast_list = mv643xx_eth_set_rx_mode;
+       dev->set_rx_mode = mv643xx_eth_set_rx_mode;
        dev->set_mac_address = mv643xx_eth_set_mac_address;
        dev->do_ioctl = mv643xx_eth_ioctl;
        dev->change_mtu = mv643xx_eth_change_mtu;