1 files changed, 428 insertions, 142 deletions

diff --git a/drivers/net/via-rhine.c b/drivers/net/via-rhine.c
index 4930f9dbc493..7f23ab913fd9 100644
--- a/drivers/net/via-rhine.c
+++ b/drivers/net/via-rhine.c
@@ -29,14 +29,17 @@
 
 */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #define DRV_NAME        "via-rhine"
-#define DRV_VERSION     "1.4.3"
-#define DRV_RELDATE     "2007-03-06"
+#define DRV_VERSION     "1.5.0"
+#define DRV_RELDATE     "2010-10-09"
 
 
 /* A few user-configurable values.
    These may be modified when a driver module is loaded. */
 
+#define DEBUG
 static int debug = 1;   /* 1 normal messages, 0 quiet .. 7 verbose. */
 static int max_interrupt_work = 20;
 
@@ -100,6 +103,7 @@ static const int multicast_filter_limit = 32;
 #include <linux/mii.h>
 #include <linux/ethtool.h>
 #include <linux/crc32.h>
+#include <linux/if_vlan.h>
 #include <linux/bitops.h>
 #include <linux/workqueue.h>
 #include <asm/processor.h>      /* Processor type for cache alignment. */
@@ -110,8 +114,7 @@ static const int multicast_filter_limit = 32;
 
 /* These identify the driver base version and may not be removed. */
 static const char version[] __devinitconst =
-        KERN_INFO DRV_NAME ".c:v1.10-LK" DRV_VERSION " " DRV_RELDATE
-        " Written by Donald Becker\n";
+        "v1.10-LK" DRV_VERSION " " DRV_RELDATE " Written by Donald Becker";
 
 /* This driver was written to use PCI memory space. Some early versions
    of the Rhine may only work correctly with I/O space accesses. */
@@ -133,6 +136,9 @@ MODULE_PARM_DESC(debug, "VIA Rhine debug level (0-7)");
 MODULE_PARM_DESC(rx_copybreak, "VIA Rhine copy breakpoint for copy-only-tiny-frames");
 MODULE_PARM_DESC(avoid_D3, "Avoid power state D3 (work-around for broken BIOSes)");
 
+#define MCAM_SIZE       32
+#define VCAM_SIZE       32
+
 /*
                 Theory of Operation
 
@@ -279,15 +285,16 @@ MODULE_DEVICE_TABLE(pci, rhine_pci_tbl);
 /* Offsets to the device registers. */
 enum register_offsets {
         StationAddr=0x00, RxConfig=0x06, TxConfig=0x07, ChipCmd=0x08,
-        ChipCmd1=0x09,
+        ChipCmd1=0x09, TQWake=0x0A,
         IntrStatus=0x0C, IntrEnable=0x0E,
         MulticastFilter0=0x10, MulticastFilter1=0x14,
         RxRingPtr=0x18, TxRingPtr=0x1C, GFIFOTest=0x54,
-        MIIPhyAddr=0x6C, MIIStatus=0x6D, PCIBusConfig=0x6E,
+        MIIPhyAddr=0x6C, MIIStatus=0x6D, PCIBusConfig=0x6E, PCIBusConfig1=0x6F,
         MIICmd=0x70, MIIRegAddr=0x71, MIIData=0x72, MACRegEEcsr=0x74,
         ConfigA=0x78, ConfigB=0x79, ConfigC=0x7A, ConfigD=0x7B,
         RxMissed=0x7C, RxCRCErrs=0x7E, MiscCmd=0x81,
         StickyHW=0x83, IntrStatus2=0x84,
+        CamMask=0x88, CamCon=0x92, CamAddr=0x93,
         WOLcrSet=0xA0, PwcfgSet=0xA1, WOLcgSet=0xA3, WOLcrClr=0xA4,
         WOLcrClr1=0xA6, WOLcgClr=0xA7,
         PwrcsrSet=0xA8, PwrcsrSet1=0xA9, PwrcsrClr=0xAC, PwrcsrClr1=0xAD,
@@ -299,6 +306,40 @@ enum backoff_bits {
         BackCaptureEffect=0x04, BackRandom=0x08
 };
 
+/* Bits in the TxConfig (TCR) register */
+enum tcr_bits {
+        TCR_PQEN=0x01,
+        TCR_LB0=0x02,           /* loopback[0] */
+        TCR_LB1=0x04,           /* loopback[1] */
+        TCR_OFSET=0x08,
+        TCR_RTGOPT=0x10,
+        TCR_RTFT0=0x20,
+        TCR_RTFT1=0x40,
+        TCR_RTSF=0x80,
+};
+
+/* Bits in the CamCon (CAMC) register */
+enum camcon_bits {
+        CAMC_CAMEN=0x01,
+        CAMC_VCAMSL=0x02,
+        CAMC_CAMWR=0x04,
+        CAMC_CAMRD=0x08,
+};
+
+/* Bits in the PCIBusConfig1 (BCR1) register */
+enum bcr1_bits {
+        BCR1_POT0=0x01,
+        BCR1_POT1=0x02,
+        BCR1_POT2=0x04,
+        BCR1_CTFT0=0x08,
+        BCR1_CTFT1=0x10,
+        BCR1_CTSF=0x20,
+        BCR1_TXQNOBK=0x40,      /* for VT6105 */
+        BCR1_VIDFR=0x80,        /* for VT6105 */
+        BCR1_MED0=0x40,         /* for VT6102 */
+        BCR1_MED1=0x80,         /* for VT6102 */
+};
+
 #ifdef USE_MMIO
 /* Registers we check that mmio and reg are the same. */
 static const int mmio_verify_registers[] = {
@@ -356,6 +397,11 @@ enum desc_status_bits {
         DescOwn=0x80000000
 };
 
+/* Bits in *_desc.*_length */
+enum desc_length_bits {
+        DescTag=0x00010000
+};
+
 /* Bits in ChipCmd. */
 enum chip_cmd_bits {
         CmdInit=0x01, CmdStart=0x02, CmdStop=0x04, CmdRxOn=0x08,
@@ -365,6 +411,9 @@ enum chip_cmd_bits {
 };
 
 struct rhine_private {
+        /* Bit mask for configured VLAN ids */
+        unsigned long active_vlans[BITS_TO_LONGS(VLAN_N_VID)];
+
         /* Descriptor rings */
         struct rx_desc *rx_ring;
         struct tx_desc *tx_ring;
@@ -405,6 +454,23 @@ struct rhine_private {
         void __iomem *base;
 };
 
+#define BYTE_REG_BITS_ON(x, p)      do { iowrite8((ioread8((p))|(x)), (p)); } while (0)
+#define WORD_REG_BITS_ON(x, p)      do { iowrite16((ioread16((p))|(x)), (p)); } while (0)
+#define DWORD_REG_BITS_ON(x, p)     do { iowrite32((ioread32((p))|(x)), (p)); } while (0)
+
+#define BYTE_REG_BITS_IS_ON(x, p)   (ioread8((p)) & (x))
+#define WORD_REG_BITS_IS_ON(x, p)   (ioread16((p)) & (x))
+#define DWORD_REG_BITS_IS_ON(x, p)  (ioread32((p)) & (x))
+
+#define BYTE_REG_BITS_OFF(x, p)     do { iowrite8(ioread8((p)) & (~(x)), (p)); } while (0)
+#define WORD_REG_BITS_OFF(x, p)     do { iowrite16(ioread16((p)) & (~(x)), (p)); } while (0)
+#define DWORD_REG_BITS_OFF(x, p)    do { iowrite32(ioread32((p)) & (~(x)), (p)); } while (0)
+
+#define BYTE_REG_BITS_SET(x, m, p)   do { iowrite8((ioread8((p)) & (~(m)))|(x), (p)); } while (0)
+#define WORD_REG_BITS_SET(x, m, p)   do { iowrite16((ioread16((p)) & (~(m)))|(x), (p)); } while (0)
+#define DWORD_REG_BITS_SET(x, m, p)  do { iowrite32((ioread32((p)) & (~(m)))|(x), (p)); } while (0)
+
+
 static int  mdio_read(struct net_device *dev, int phy_id, int location);
 static void mdio_write(struct net_device *dev, int phy_id, int location, int value);
 static int  rhine_open(struct net_device *dev);
@@ -422,15 +488,24 @@ static int netdev_ioctl(struct net_device *dev, struct ifreq *rq, int cmd);
 static const struct ethtool_ops netdev_ethtool_ops;
 static int  rhine_close(struct net_device *dev);
 static void rhine_shutdown (struct pci_dev *pdev);
-
-#define RHINE_WAIT_FOR(condition) do {                                  \
-        int i=1024;                                                     \
-        while (!(condition) && --i)                                     \
-                ;                                                       \
-        if (debug > 1 && i < 512)                                       \
-                printk(KERN_INFO "%s: %4d cycles used @ %s:%d\n",       \
-                                DRV_NAME, 1024-i, __func__, __LINE__);  \
-} while(0)
+static void rhine_vlan_rx_add_vid(struct net_device *dev, unsigned short vid);
+static void rhine_vlan_rx_kill_vid(struct net_device *dev, unsigned short vid);
+static void rhine_set_cam(void __iomem *ioaddr, int idx, u8 *addr);
+static void rhine_set_vlan_cam(void __iomem *ioaddr, int idx, u8 *addr);
+static void rhine_set_cam_mask(void __iomem *ioaddr, u32 mask);
+static void rhine_set_vlan_cam_mask(void __iomem *ioaddr, u32 mask);
+static void rhine_init_cam_filter(struct net_device *dev);
+static void rhine_update_vcam(struct net_device *dev);
+
+#define RHINE_WAIT_FOR(condition)                               \
+do {                                                            \
+        int i = 1024;                                           \
+        while (!(condition) && --i)                             \
+                ;                                               \
+        if (debug > 1 && i < 512)                               \
+                pr_info("%4d cycles used @ %s:%d\n",            \
+                        1024 - i, __func__, __LINE__);          \
+} while (0)
 
 static inline u32 get_intr_status(struct net_device *dev)
 {
@@ -499,8 +574,8 @@ static void rhine_power_init(struct net_device *dev)
                         default:
                                 reason = "Unknown";
                         }
-                        printk(KERN_INFO "%s: Woke system up. Reason: %s.\n",
-                               DRV_NAME, reason);
+                        netdev_info(dev, "Woke system up. Reason: %s\n",
+                                    reason);
                 }
         }
 }
@@ -514,8 +589,7 @@ static void rhine_chip_reset(struct net_device *dev)
         IOSYNC;
 
         if (ioread8(ioaddr + ChipCmd1) & Cmd1Reset) {
-                printk(KERN_INFO "%s: Reset not complete yet. "
-                        "Trying harder.\n", DRV_NAME);
+                netdev_info(dev, "Reset not complete yet. Trying harder.\n");
 
                 /* Force reset */
                 if (rp->quirks & rqForceReset)
@@ -526,9 +600,9 @@ static void rhine_chip_reset(struct net_device *dev)
         }
 
         if (debug > 1)
-                printk(KERN_INFO "%s: Reset %s.\n", dev->name,
-                        (ioread8(ioaddr + ChipCmd1) & Cmd1Reset) ?
-                        "failed" : "succeeded");
+                netdev_info(dev, "Reset %s\n",
+                            (ioread8(ioaddr + ChipCmd1) & Cmd1Reset) ?
+                            "failed" : "succeeded");
 }
 
 #ifdef USE_MMIO
@@ -629,6 +703,8 @@ static const struct net_device_ops rhine_netdev_ops = {
         .ndo_set_mac_address     = eth_mac_addr,
         .ndo_do_ioctl            = netdev_ioctl,
         .ndo_tx_timeout          = rhine_tx_timeout,
+        .ndo_vlan_rx_add_vid     = rhine_vlan_rx_add_vid,
+        .ndo_vlan_rx_kill_vid    = rhine_vlan_rx_kill_vid,
 #ifdef CONFIG_NET_POLL_CONTROLLER
         .ndo_poll_controller     = rhine_poll,
 #endif
@@ -654,9 +730,7 @@ static int __devinit rhine_init_one(struct pci_dev *pdev,
 
 /* when built into the kernel, we only print version if device is found */
 #ifndef MODULE
-        static int printed_version;
-        if (!printed_version++)
-                printk(version);
+        pr_info_once("%s\n", version);
 #endif
 
         io_size = 256;
@@ -691,8 +765,8 @@ static int __devinit rhine_init_one(struct pci_dev *pdev,
         /* this should always be supported */
         rc = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
         if (rc) {
-                printk(KERN_ERR "32-bit PCI DMA addresses not supported by "
-                       "the card!?\n");
+                dev_err(&pdev->dev,
+                        "32-bit PCI DMA addresses not supported by the card!?\n");
                 goto err_out;
         }
 
@@ -700,7 +774,7 @@ static int __devinit rhine_init_one(struct pci_dev *pdev,
         if ((pci_resource_len(pdev, 0) < io_size) ||
             (pci_resource_len(pdev, 1) < io_size)) {
                 rc = -EIO;
-                printk(KERN_ERR "Insufficient PCI resources, aborting\n");
+                dev_err(&pdev->dev, "Insufficient PCI resources, aborting\n");
                 goto err_out;
         }
 
@@ -712,7 +786,7 @@ static int __devinit rhine_init_one(struct pci_dev *pdev,
         dev = alloc_etherdev(sizeof(struct rhine_private));
         if (!dev) {
                 rc = -ENOMEM;
-                printk(KERN_ERR "alloc_etherdev failed\n");
+                dev_err(&pdev->dev, "alloc_etherdev failed\n");
                 goto err_out;
         }
         SET_NETDEV_DEV(dev, &pdev->dev);
@@ -730,8 +804,9 @@ static int __devinit rhine_init_one(struct pci_dev *pdev,
         ioaddr = pci_iomap(pdev, bar, io_size);
         if (!ioaddr) {
                 rc = -EIO;
-                printk(KERN_ERR "ioremap failed for device %s, region 0x%X "
-                       "@ 0x%lX\n", pci_name(pdev), io_size, memaddr);
+                dev_err(&pdev->dev,
+                        "ioremap failed for device %s, region 0x%X @ 0x%lX\n",
+                        pci_name(pdev), io_size, memaddr);
                 goto err_out_free_res;
         }
 
@@ -746,8 +821,9 @@ static int __devinit rhine_init_one(struct pci_dev *pdev,
                 unsigned char b = readb(ioaddr+reg);
                 if (a != b) {
                         rc = -EIO;
-                        printk(KERN_ERR "MMIO do not match PIO [%02x] "
-                               "(%02x != %02x)\n", reg, a, b);
+                        dev_err(&pdev->dev,
+                                "MMIO do not match PIO [%02x] (%02x != %02x)\n",
+                                reg, a, b);
                         goto err_out_unmap;
                 }
         }
@@ -762,13 +838,15 @@ static int __devinit rhine_init_one(struct pci_dev *pdev,
 
         for (i = 0; i < 6; i++)
                 dev->dev_addr[i] = ioread8(ioaddr + StationAddr + i);
-        memcpy(dev->perm_addr, dev->dev_addr, dev->addr_len);
 
-        if (!is_valid_ether_addr(dev->perm_addr)) {
-                rc = -EIO;
-                printk(KERN_ERR "Invalid MAC address\n");
-                goto err_out_unmap;
+        if (!is_valid_ether_addr(dev->dev_addr)) {
+                /* Report it and use a random ethernet address instead */
+                netdev_err(dev, "Invalid MAC address: %pM\n", dev->dev_addr);
+                random_ether_addr(dev->dev_addr);
+                netdev_info(dev, "Using random MAC address: %pM\n",
+                            dev->dev_addr);
         }
+        memcpy(dev->perm_addr, dev->dev_addr, dev->addr_len);
 
         /* For Rhine-I/II, phy_id is loaded from EEPROM */
         if (!phy_id)
@@ -795,19 +873,23 @@ static int __devinit rhine_init_one(struct pci_dev *pdev,
         if (rp->quirks & rqRhineI)
                 dev->features |= NETIF_F_SG|NETIF_F_HW_CSUM;
 
+        if (pdev->revision >= VT6105M)
+                dev->features |= NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX |
+                NETIF_F_HW_VLAN_FILTER;
+
         /* dev->name not defined before register_netdev()! */
         rc = register_netdev(dev);
         if (rc)
                 goto err_out_unmap;
 
-        printk(KERN_INFO "%s: VIA %s at 0x%lx, %pM, IRQ %d.\n",
-               dev->name, name,
+        netdev_info(dev, "VIA %s at 0x%lx, %pM, IRQ %d\n",
+                    name,
 #ifdef USE_MMIO
-               memaddr,
+                    memaddr,
 #else
-               (long)ioaddr,
+                    (long)ioaddr,
 #endif
-               dev->dev_addr, pdev->irq);
+                    dev->dev_addr, pdev->irq);
 
         pci_set_drvdata(pdev, dev);
 
@@ -818,11 +900,11 @@ static int __devinit rhine_init_one(struct pci_dev *pdev,
                 mdio_write(dev, phy_id, MII_BMCR, mii_cmd);
                 if (mii_status != 0xffff && mii_status != 0x0000) {
                         rp->mii_if.advertising = mdio_read(dev, phy_id, 4);
-                        printk(KERN_INFO "%s: MII PHY found at address "
-                               "%d, status 0x%4.4x advertising %4.4x "
-                               "Link %4.4x.\n", dev->name, phy_id,
-                               mii_status, rp->mii_if.advertising,
-                               mdio_read(dev, phy_id, 5));
+                        netdev_info(dev,
+                                    "MII PHY found at address %d, status 0x%04x advertising %04x Link %04x\n",
+                                    phy_id,
+                                    mii_status, rp->mii_if.advertising,
+                                    mdio_read(dev, phy_id, 5));
 
                         /* set IFF_RUNNING */
                         if (mii_status & BMSR_LSTATUS)
@@ -834,8 +916,7 @@ static int __devinit rhine_init_one(struct pci_dev *pdev,
         }
         rp->mii_if.phy_id = phy_id;
         if (debug > 1 && avoid_D3)
-                printk(KERN_INFO "%s: No D3 power state at shutdown.\n",
-                       dev->name);
+                netdev_info(dev, "No D3 power state at shutdown\n");
 
         return 0;
 
@@ -860,7 +941,7 @@ static int alloc_ring(struct net_device* dev)
                                     TX_RING_SIZE * sizeof(struct tx_desc),
                                     &ring_dma);
         if (!ring) {
-                printk(KERN_ERR "Could not allocate DMA memory.\n");
+                netdev_err(dev, "Could not allocate DMA memory\n");
                 return -ENOMEM;
         }
         if (rp->quirks & rqRhineI) {
@@ -1020,8 +1101,8 @@ static void rhine_check_media(struct net_device *dev, unsigned int init_media)
             iowrite8(ioread8(ioaddr + ChipCmd1) & ~Cmd1FDuplex,
                    ioaddr + ChipCmd1);
         if (debug > 1)
-                printk(KERN_INFO "%s: force_media %d, carrier %d\n", dev->name,
-                        rp->mii_if.force_media, netif_carrier_ok(dev));
+                netdev_info(dev, "force_media %d, carrier %d\n",
+                            rp->mii_if.force_media, netif_carrier_ok(dev));
 }
 
 /* Called after status of force_media possibly changed */
@@ -1035,9 +1116,169 @@ static void rhine_set_carrier(struct mii_if_info *mii)
         else    /* Let MMI library update carrier status */
                 rhine_check_media(mii->dev, 0);
         if (debug > 1)
-                printk(KERN_INFO "%s: force_media %d, carrier %d\n",
-                       mii->dev->name, mii->force_media,
-                       netif_carrier_ok(mii->dev));
+                netdev_info(mii->dev, "force_media %d, carrier %d\n",
+                            mii->force_media, netif_carrier_ok(mii->dev));
+}
+
+/**
+ * rhine_set_cam - set CAM multicast filters
+ * @ioaddr: register block of this Rhine
+ * @idx: multicast CAM index [0..MCAM_SIZE-1]
+ * @addr: multicast address (6 bytes)
+ *
+ * Load addresses into multicast filters.
+ */
+static void rhine_set_cam(void __iomem *ioaddr, int idx, u8 *addr)
+{
+        int i;
+
+        iowrite8(CAMC_CAMEN, ioaddr + CamCon);
+        wmb();
+
+        /* Paranoid -- idx out of range should never happen */
+        idx &= (MCAM_SIZE - 1);
+
+        iowrite8((u8) idx, ioaddr + CamAddr);
+
+        for (i = 0; i < 6; i++, addr++)
+                iowrite8(*addr, ioaddr + MulticastFilter0 + i);
+        udelay(10);
+        wmb();
+
+        iowrite8(CAMC_CAMWR | CAMC_CAMEN, ioaddr + CamCon);
+        udelay(10);
+
+        iowrite8(0, ioaddr + CamCon);
+}
+
+/**
+ * rhine_set_vlan_cam - set CAM VLAN filters
+ * @ioaddr: register block of this Rhine
+ * @idx: VLAN CAM index [0..VCAM_SIZE-1]
+ * @addr: VLAN ID (2 bytes)
+ *
+ * Load addresses into VLAN filters.
+ */
+static void rhine_set_vlan_cam(void __iomem *ioaddr, int idx, u8 *addr)
+{
+        iowrite8(CAMC_CAMEN | CAMC_VCAMSL, ioaddr + CamCon);
+        wmb();
+
+        /* Paranoid -- idx out of range should never happen */
+        idx &= (VCAM_SIZE - 1);
+
+        iowrite8((u8) idx, ioaddr + CamAddr);
+
+        iowrite16(*((u16 *) addr), ioaddr + MulticastFilter0 + 6);
+        udelay(10);
+        wmb();
+
+        iowrite8(CAMC_CAMWR | CAMC_CAMEN, ioaddr + CamCon);
+        udelay(10);
+
+        iowrite8(0, ioaddr + CamCon);
+}
+
+/**
+ * rhine_set_cam_mask - set multicast CAM mask
+ * @ioaddr: register block of this Rhine
+ * @mask: multicast CAM mask
+ *
+ * Mask sets multicast filters active/inactive.
+ */
+static void rhine_set_cam_mask(void __iomem *ioaddr, u32 mask)
+{
+        iowrite8(CAMC_CAMEN, ioaddr + CamCon);
+        wmb();
+
+        /* write mask */
+        iowrite32(mask, ioaddr + CamMask);
+
+        /* disable CAMEN */
+        iowrite8(0, ioaddr + CamCon);
+}
+
+/**
+ * rhine_set_vlan_cam_mask - set VLAN CAM mask
+ * @ioaddr: register block of this Rhine
+ * @mask: VLAN CAM mask
+ *
+ * Mask sets VLAN filters active/inactive.
+ */
+static void rhine_set_vlan_cam_mask(void __iomem *ioaddr, u32 mask)
+{
+        iowrite8(CAMC_CAMEN | CAMC_VCAMSL, ioaddr + CamCon);
+        wmb();
+
+        /* write mask */
+        iowrite32(mask, ioaddr + CamMask);
+
+        /* disable CAMEN */
+        iowrite8(0, ioaddr + CamCon);
+}
+
+/**
+ * rhine_init_cam_filter - initialize CAM filters
+ * @dev: network device
+ *
+ * Initialize (disable) hardware VLAN and multicast support on this
+ * Rhine.
+ */
+static void rhine_init_cam_filter(struct net_device *dev)
+{
+        struct rhine_private *rp = netdev_priv(dev);
+        void __iomem *ioaddr = rp->base;
+
+        /* Disable all CAMs */
+        rhine_set_vlan_cam_mask(ioaddr, 0);
+        rhine_set_cam_mask(ioaddr, 0);
+
+        /* disable hardware VLAN support */
+        BYTE_REG_BITS_ON(TCR_PQEN, ioaddr + TxConfig);
+        BYTE_REG_BITS_OFF(BCR1_VIDFR, ioaddr + PCIBusConfig1);
+}
+
+/**
+ * rhine_update_vcam - update VLAN CAM filters
+ * @rp: rhine_private data of this Rhine
+ *
+ * Update VLAN CAM filters to match configuration change.
+ */
+static void rhine_update_vcam(struct net_device *dev)
+{
+        struct rhine_private *rp = netdev_priv(dev);
+        void __iomem *ioaddr = rp->base;
+        u16 vid;
+        u32 vCAMmask = 0;       /* 32 vCAMs (6105M and better) */
+        unsigned int i = 0;
+
+        for_each_set_bit(vid, rp->active_vlans, VLAN_N_VID) {
+                rhine_set_vlan_cam(ioaddr, i, (u8 *)&vid);
+                vCAMmask |= 1 << i;
+                if (++i >= VCAM_SIZE)
+                        break;
+        }
+        rhine_set_vlan_cam_mask(ioaddr, vCAMmask);
+}
+
+static void rhine_vlan_rx_add_vid(struct net_device *dev, unsigned short vid)
+{
+        struct rhine_private *rp = netdev_priv(dev);
+
+        spin_lock_irq(&rp->lock);
+        set_bit(vid, rp->active_vlans);
+        rhine_update_vcam(dev);
+        spin_unlock_irq(&rp->lock);
+}
+
+static void rhine_vlan_rx_kill_vid(struct net_device *dev, unsigned short vid)
+{
+        struct rhine_private *rp = netdev_priv(dev);
+
+        spin_lock_irq(&rp->lock);
+        clear_bit(vid, rp->active_vlans);
+        rhine_update_vcam(dev);
+        spin_unlock_irq(&rp->lock);
 }
 
 static void init_registers(struct net_device *dev)
@@ -1061,6 +1302,9 @@ static void init_registers(struct net_device *dev)
 
         rhine_set_rx_mode(dev);
 
+        if (rp->pdev->revision >= VT6105M)
+                rhine_init_cam_filter(dev);
+
         napi_enable(&rp->napi);
 
         /* Enable interrupts by setting the interrupt mask. */
@@ -1160,8 +1404,7 @@ static int rhine_open(struct net_device *dev)
                 return rc;
 
         if (debug > 1)
-                printk(KERN_DEBUG "%s: rhine_open() irq %d.\n",
-                       dev->name, rp->pdev->irq);
+                netdev_dbg(dev, "%s() irq %d\n", __func__, rp->pdev->irq);
 
         rc = alloc_ring(dev);
         if (rc) {
@@ -1173,10 +1416,9 @@ static int rhine_open(struct net_device *dev)
         rhine_chip_reset(dev);
         init_registers(dev);
         if (debug > 2)
-                printk(KERN_DEBUG "%s: Done rhine_open(), status %4.4x "
-                       "MII status: %4.4x.\n",
-                       dev->name, ioread16(ioaddr + ChipCmd),
-                       mdio_read(dev, rp->mii_if.phy_id, MII_BMSR));
+                netdev_dbg(dev, "%s() Done - status %04x MII status: %04x\n",
+                           __func__, ioread16(ioaddr + ChipCmd),
+                           mdio_read(dev, rp->mii_if.phy_id, MII_BMSR));
 
         netif_start_queue(dev);
 
@@ -1219,10 +1461,9 @@ static void rhine_tx_timeout(struct net_device *dev)
         struct rhine_private *rp = netdev_priv(dev);
         void __iomem *ioaddr = rp->base;
 
-        printk(KERN_WARNING "%s: Transmit timed out, status %4.4x, PHY status "
-               "%4.4x, resetting...\n",
-               dev->name, ioread16(ioaddr + IntrStatus),
-               mdio_read(dev, rp->mii_if.phy_id, MII_BMSR));
+        netdev_warn(dev, "Transmit timed out, status %04x, PHY status %04x, resetting...\n",
+                    ioread16(ioaddr + IntrStatus),
+                    mdio_read(dev, rp->mii_if.phy_id, MII_BMSR));
 
         schedule_work(&rp->reset_task);
 }
@@ -1276,16 +1517,28 @@ static netdev_tx_t rhine_start_tx(struct sk_buff *skb,
         rp->tx_ring[entry].desc_length =
                 cpu_to_le32(TXDESC | (skb->len >= ETH_ZLEN ? skb->len : ETH_ZLEN));
 
+        if (unlikely(vlan_tx_tag_present(skb))) {
+                rp->tx_ring[entry].tx_status = cpu_to_le32((vlan_tx_tag_get(skb)) << 16);
+                /* request tagging */
+                rp->tx_ring[entry].desc_length |= cpu_to_le32(0x020000);
+        }
+        else
+                rp->tx_ring[entry].tx_status = 0;
+
         /* lock eth irq */
         spin_lock_irqsave(&rp->lock, flags);
         wmb();
-        rp->tx_ring[entry].tx_status = cpu_to_le32(DescOwn);
+        rp->tx_ring[entry].tx_status |= cpu_to_le32(DescOwn);
         wmb();
 
         rp->cur_tx++;
 
         /* Non-x86 Todo: explicitly flush cache lines here. */
 
+        if (vlan_tx_tag_present(skb))
+                /* Tx queues are bits 7-0 (first Tx queue: bit 7) */
+                BYTE_REG_BITS_ON(1 << 7, ioaddr + TQWake);
+
         /* Wake the potentially-idle transmit channel */
         iowrite8(ioread8(ioaddr + ChipCmd1) | Cmd1TxDemand,
                ioaddr + ChipCmd1);
@@ -1297,8 +1550,8 @@ static netdev_tx_t rhine_start_tx(struct sk_buff *skb,
         spin_unlock_irqrestore(&rp->lock, flags);
 
         if (debug > 4) {
-                printk(KERN_DEBUG "%s: Transmit frame #%d queued in slot %d.\n",
-                       dev->name, rp->cur_tx-1, entry);
+                netdev_dbg(dev, "Transmit frame #%d queued in slot %d\n",
+                           rp->cur_tx-1, entry);
         }
         return NETDEV_TX_OK;
 }
@@ -1324,8 +1577,8 @@ static irqreturn_t rhine_interrupt(int irq, void *dev_instance)
                 IOSYNC;
 
                 if (debug > 4)
-                        printk(KERN_DEBUG "%s: Interrupt, status %8.8x.\n",
-                               dev->name, intr_status);
+                        netdev_dbg(dev, "Interrupt, status %08x\n",
+                                   intr_status);
 
                 if (intr_status & (IntrRxDone | IntrRxErr | IntrRxDropped |
                                    IntrRxWakeUp | IntrRxEmpty | IntrRxNoBuf)) {
@@ -1343,9 +1596,9 @@ static irqreturn_t rhine_interrupt(int irq, void *dev_instance)
                                 RHINE_WAIT_FOR(!(ioread8(ioaddr+ChipCmd) & CmdTxOn));
                                 if (debug > 2 &&
                                     ioread8(ioaddr+ChipCmd) & CmdTxOn)
-                                        printk(KERN_WARNING "%s: "
-                                               "rhine_interrupt() Tx engine "
-                                               "still on.\n", dev->name);
+                                        netdev_warn(dev,
+                                                    "%s: Tx engine still on\n",
+                                                    __func__);
                         }
                         rhine_tx(dev);
                 }
@@ -1357,16 +1610,15 @@ static irqreturn_t rhine_interrupt(int irq, void *dev_instance)
                         rhine_error(dev, intr_status);
 
                 if (--boguscnt < 0) {
-                        printk(KERN_WARNING "%s: Too much work at interrupt, "
-                               "status=%#8.8x.\n",
-                               dev->name, intr_status);
+                        netdev_warn(dev, "Too much work at interrupt, status=%#08x\n",
+                                    intr_status);
                         break;
                 }
         }
 
         if (debug > 3)
-                printk(KERN_DEBUG "%s: exiting interrupt, status=%8.8x.\n",
-                       dev->name, ioread16(ioaddr + IntrStatus));
+                netdev_dbg(dev, "exiting interrupt, status=%08x\n",
+                           ioread16(ioaddr + IntrStatus));
         return IRQ_RETVAL(handled);
 }
 
@@ -1383,15 +1635,14 @@ static void rhine_tx(struct net_device *dev)
         while (rp->dirty_tx != rp->cur_tx) {
                 txstatus = le32_to_cpu(rp->tx_ring[entry].tx_status);
                 if (debug > 6)
-                        printk(KERN_DEBUG "Tx scavenge %d status %8.8x.\n",
-                               entry, txstatus);
+                        netdev_dbg(dev, "Tx scavenge %d status %08x\n",
+                                   entry, txstatus);
                 if (txstatus & DescOwn)
                         break;
                 if (txstatus & 0x8000) {
                         if (debug > 1)
-                                printk(KERN_DEBUG "%s: Transmit error, "
-                                       "Tx status %8.8x.\n",
-                                       dev->name, txstatus);
+                                netdev_dbg(dev, "Transmit error, Tx status %08x\n",
+                                           txstatus);
                         dev->stats.tx_errors++;
                         if (txstatus & 0x0400)
                                 dev->stats.tx_carrier_errors++;
@@ -1414,9 +1665,9 @@ static void rhine_tx(struct net_device *dev)
                         else
                                 dev->stats.collisions += txstatus & 0x0F;
                         if (debug > 6)
-                                printk(KERN_DEBUG "collisions: %1.1x:%1.1x\n",
-                                       (txstatus >> 3) & 0xF,
-                                       txstatus & 0xF);
+                                netdev_dbg(dev, "collisions: %1.1x:%1.1x\n",
+                                           (txstatus >> 3) & 0xF,
+                                           txstatus & 0xF);
                         dev->stats.tx_bytes += rp->tx_skbuff[entry]->len;
                         dev->stats.tx_packets++;
                 }
@@ -1437,6 +1688,21 @@ static void rhine_tx(struct net_device *dev)
         spin_unlock(&rp->lock);
 }
 
+/**
+ * rhine_get_vlan_tci - extract TCI from Rx data buffer
+ * @skb: pointer to sk_buff
+ * @data_size: used data area of the buffer including CRC
+ *
+ * If hardware VLAN tag extraction is enabled and the chip indicates a 802.1Q
+ * packet, the extracted 802.1Q header (2 bytes TPID + 2 bytes TCI) is 4-byte
+ * aligned following the CRC.
+ */
+static inline u16 rhine_get_vlan_tci(struct sk_buff *skb, int data_size)
+{
+        u8 *trailer = (u8 *)skb->data + ((data_size + 3) & ~3) + 2;
+        return be16_to_cpup((__be16 *)trailer);
+}
+
 /* Process up to limit frames from receive ring */
 static int rhine_rx(struct net_device *dev, int limit)
 {
@@ -1445,41 +1711,42 @@ static int rhine_rx(struct net_device *dev, int limit)
         int entry = rp->cur_rx % RX_RING_SIZE;
 
         if (debug > 4) {
-                printk(KERN_DEBUG "%s: rhine_rx(), entry %d status %8.8x.\n",
-                       dev->name, entry,
-                       le32_to_cpu(rp->rx_head_desc->rx_status));
+                netdev_dbg(dev, "%s(), entry %d status %08x\n",
+                           __func__, entry,
+                           le32_to_cpu(rp->rx_head_desc->rx_status));
         }
 
         /* If EOP is set on the next entry, it's a new packet. Send it up. */
         for (count = 0; count < limit; ++count) {
                 struct rx_desc *desc = rp->rx_head_desc;
                 u32 desc_status = le32_to_cpu(desc->rx_status);
+                u32 desc_length = le32_to_cpu(desc->desc_length);
                 int data_size = desc_status >> 16;
 
                 if (desc_status & DescOwn)
                         break;
 
                 if (debug > 4)
-                        printk(KERN_DEBUG "rhine_rx() status is %8.8x.\n",
-                               desc_status);
+                        netdev_dbg(dev, "%s() status is %08x\n",
+                                   __func__, desc_status);
 
                 if ((desc_status & (RxWholePkt | RxErr)) != RxWholePkt) {
                         if ((desc_status & RxWholePkt) != RxWholePkt) {
-                                printk(KERN_WARNING "%s: Oversized Ethernet "
-                                       "frame spanned multiple buffers, entry "
-                                       "%#x length %d status %8.8x!\n",
-                                       dev->name, entry, data_size,
-                                       desc_status);
-                                printk(KERN_WARNING "%s: Oversized Ethernet "
-                                       "frame %p vs %p.\n", dev->name,
-                                       rp->rx_head_desc, &rp->rx_ring[entry]);
+                                netdev_warn(dev,
+        "Oversized Ethernet frame spanned multiple buffers, "
+        "entry %#x length %d status %08x!\n",
+                                            entry, data_size,
+                                            desc_status);
+                                netdev_warn(dev,
+                                            "Oversized Ethernet frame %p vs %p\n",
+                                            rp->rx_head_desc,
+                                            &rp->rx_ring[entry]);
                                 dev->stats.rx_length_errors++;
                         } else if (desc_status & RxErr) {
                                 /* There was a error. */
                                 if (debug > 2)
-                                        printk(KERN_DEBUG "rhine_rx() Rx "
-                                               "error was %8.8x.\n",
-                                               desc_status);
+                                        netdev_dbg(dev, "%s() Rx error was %08x\n",
+                                                   __func__, desc_status);
                                 dev->stats.rx_errors++;
                                 if (desc_status & 0x0030)
                                         dev->stats.rx_length_errors++;
@@ -1498,6 +1765,7 @@ static int rhine_rx(struct net_device *dev, int limit)
                         struct sk_buff *skb = NULL;
                         /* Length should omit the CRC */
                         int pkt_len = data_size - 4;
+                        u16 vlan_tci = 0;
 
                         /* Check if the packet is long enough to accept without
                            copying to a minimally-sized skbuff. */
@@ -1520,9 +1788,7 @@ static int rhine_rx(struct net_device *dev, int limit)
                         } else {
                                 skb = rp->rx_skbuff[entry];
                                 if (skb == NULL) {
-                                        printk(KERN_ERR "%s: Inconsistent Rx "
-                                               "descriptor chain.\n",
-                                               dev->name);
+                                        netdev_err(dev, "Inconsistent Rx descriptor chain\n");
                                         break;
                                 }
                                 rp->rx_skbuff[entry] = NULL;
@@ -1532,7 +1798,14 @@ static int rhine_rx(struct net_device *dev, int limit)
                                                  rp->rx_buf_sz,
                                                  PCI_DMA_FROMDEVICE);
                         }
+
+                        if (unlikely(desc_length & DescTag))
+                                vlan_tci = rhine_get_vlan_tci(skb, data_size);
+
                         skb->protocol = eth_type_trans(skb, dev);
+
+                        if (unlikely(desc_length & DescTag))
+                                __vlan_hwaccel_put_tag(skb, vlan_tci);
                         netif_receive_skb(skb);
                         dev->stats.rx_bytes += pkt_len;
                         dev->stats.rx_packets++;
@@ -1583,7 +1856,7 @@ static void rhine_restart_tx(struct net_device *dev) {
         u32 intr_status;
 
         /*
-         * If new errors occured, we need to sort them out before doing Tx.
+         * If new errors occurred, we need to sort them out before doing Tx.
          * In that case the ISR will be back here RSN anyway.
          */
         intr_status = get_intr_status(dev);
@@ -1596,6 +1869,11 @@ static void rhine_restart_tx(struct net_device *dev) {
 
                 iowrite8(ioread8(ioaddr + ChipCmd) | CmdTxOn,
                        ioaddr + ChipCmd);
+
+                if (rp->tx_ring[entry].desc_length & cpu_to_le32(0x020000))
+                        /* Tx queues are bits 7-0 (first Tx queue: bit 7) */
+                        BYTE_REG_BITS_ON(1 << 7, ioaddr + TQWake);
+
                 iowrite8(ioread8(ioaddr + ChipCmd1) | Cmd1TxDemand,
                        ioaddr + ChipCmd1);
                 IOSYNC;
@@ -1603,9 +1881,8 @@ static void rhine_restart_tx(struct net_device *dev) {
         else {
                 /* This should never happen */
                 if (debug > 1)
-                        printk(KERN_WARNING "%s: rhine_restart_tx() "
-                               "Another error occured %8.8x.\n",
-                               dev->name, intr_status);
+                        netdev_warn(dev, "%s() Another error occurred %08x\n",
+                                   __func__, intr_status);
         }
 
 }
@@ -1626,32 +1903,29 @@ static void rhine_error(struct net_device *dev, int intr_status)
         }
         if (intr_status & IntrTxAborted) {
                 if (debug > 1)
-                        printk(KERN_INFO "%s: Abort %8.8x, frame dropped.\n",
-                               dev->name, intr_status);
+                        netdev_info(dev, "Abort %08x, frame dropped\n",
+                                    intr_status);
         }
         if (intr_status & IntrTxUnderrun) {
                 if (rp->tx_thresh < 0xE0)
-                        iowrite8(rp->tx_thresh += 0x20, ioaddr + TxConfig);
+                        BYTE_REG_BITS_SET((rp->tx_thresh += 0x20), 0x80, ioaddr + TxConfig);
                 if (debug > 1)
-                        printk(KERN_INFO "%s: Transmitter underrun, Tx "
-                               "threshold now %2.2x.\n",
-                               dev->name, rp->tx_thresh);
+                        netdev_info(dev, "Transmitter underrun, Tx threshold now %02x\n",
+                                    rp->tx_thresh);
         }
         if (intr_status & IntrTxDescRace) {
                 if (debug > 2)
-                        printk(KERN_INFO "%s: Tx descriptor write-back race.\n",
-                               dev->name);
+                        netdev_info(dev, "Tx descriptor write-back race\n");
         }
         if ((intr_status & IntrTxError) &&
             (intr_status & (IntrTxAborted |
              IntrTxUnderrun | IntrTxDescRace)) == 0) {
                 if (rp->tx_thresh < 0xE0) {
-                        iowrite8(rp->tx_thresh += 0x20, ioaddr + TxConfig);
+                        BYTE_REG_BITS_SET((rp->tx_thresh += 0x20), 0x80, ioaddr + TxConfig);
                 }
                 if (debug > 1)
-                        printk(KERN_INFO "%s: Unspecified error. Tx "
-                               "threshold now %2.2x.\n",
-                               dev->name, rp->tx_thresh);
+                        netdev_info(dev, "Unspecified error. Tx threshold now %02x\n",
+                                    rp->tx_thresh);
         }
         if (intr_status & (IntrTxAborted | IntrTxUnderrun | IntrTxDescRace |
                            IntrTxError))
@@ -1661,8 +1935,8 @@ static void rhine_error(struct net_device *dev, int intr_status)
                             IntrTxError | IntrTxAborted | IntrNormalSummary |
                             IntrTxDescRace)) {
                 if (debug > 1)
-                        printk(KERN_ERR "%s: Something Wicked happened! "
-                               "%8.8x.\n", dev->name, intr_status);
+                        netdev_err(dev, "Something Wicked happened! %08x\n",
+                                   intr_status);
         }
 
         spin_unlock(&rp->lock);
@@ -1688,7 +1962,8 @@ static void rhine_set_rx_mode(struct net_device *dev)
         struct rhine_private *rp = netdev_priv(dev);
         void __iomem *ioaddr = rp->base;
         u32 mc_filter[2];       /* Multicast hash filter */
-        u8 rx_mode;             /* Note: 0x02=accept runt, 0x01=accept errs */
+        u8 rx_mode = 0x0C;      /* Note: 0x02=accept runt, 0x01=accept errs */
+        struct netdev_hw_addr *ha;
 
         if (dev->flags & IFF_PROMISC) {         /* Set promiscuous. */
                 rx_mode = 0x1C;
@@ -1699,10 +1974,18 @@ static void rhine_set_rx_mode(struct net_device *dev)
                 /* Too many to match, or accept all multicasts. */
                 iowrite32(0xffffffff, ioaddr + MulticastFilter0);
                 iowrite32(0xffffffff, ioaddr + MulticastFilter1);
-                rx_mode = 0x0C;
+        } else if (rp->pdev->revision >= VT6105M) {
+                int i = 0;
+                u32 mCAMmask = 0;       /* 32 mCAMs (6105M and better) */
+                netdev_for_each_mc_addr(ha, dev) {
+                        if (i == MCAM_SIZE)
+                                break;
+                        rhine_set_cam(ioaddr, i, ha->addr);
+                        mCAMmask |= 1 << i;
+                        i++;
+                }
+                rhine_set_cam_mask(ioaddr, mCAMmask);
         } else {
-                struct netdev_hw_addr *ha;
-
                 memset(mc_filter, 0, sizeof(mc_filter));
                 netdev_for_each_mc_addr(ha, dev) {
                         int bit_nr = ether_crc(ETH_ALEN, ha->addr) >> 26;
@@ -1711,9 +1994,15 @@ static void rhine_set_rx_mode(struct net_device *dev)
                 }
                 iowrite32(mc_filter[0], ioaddr + MulticastFilter0);
                 iowrite32(mc_filter[1], ioaddr + MulticastFilter1);
-                rx_mode = 0x0C;
         }
-        iowrite8(rp->rx_thresh | rx_mode, ioaddr + RxConfig);
+        /* enable/disable VLAN receive filtering */
+        if (rp->pdev->revision >= VT6105M) {
+                if (dev->flags & IFF_PROMISC)
+                        BYTE_REG_BITS_OFF(BCR1_VIDFR, ioaddr + PCIBusConfig1);
+                else
+                        BYTE_REG_BITS_ON(BCR1_VIDFR, ioaddr + PCIBusConfig1);
+        }
+        BYTE_REG_BITS_ON(rx_mode, ioaddr + RxConfig);
 }
 
 static void netdev_get_drvinfo(struct net_device *dev, struct ethtool_drvinfo *info)
@@ -1847,9 +2136,8 @@ static int rhine_close(struct net_device *dev)
         spin_lock_irq(&rp->lock);
 
         if (debug > 1)
-                printk(KERN_DEBUG "%s: Shutting down ethercard, "
-                       "status was %4.4x.\n",
-                       dev->name, ioread16(ioaddr + ChipCmd));
+                netdev_dbg(dev, "Shutting down ethercard, status was %04x\n",
+                           ioread16(ioaddr + ChipCmd));
 
         /* Switch to loopback mode to avoid hardware races. */
         iowrite8(rp->tx_thresh | 0x02, ioaddr + TxConfig);
@@ -1966,13 +2254,13 @@ static int rhine_resume(struct pci_dev *pdev)
         if (!netif_running(dev))
                 return 0;
 
-        if (request_irq(dev->irq, rhine_interrupt, IRQF_SHARED, dev->name, dev))
-                printk(KERN_ERR "via-rhine %s: request_irq failed\n", dev->name);
+        if (request_irq(dev->irq, rhine_interrupt, IRQF_SHARED, dev->name, dev))
+                netdev_err(dev, "request_irq failed\n");
 
         ret = pci_set_power_state(pdev, PCI_D0);
         if (debug > 1)
-                printk(KERN_INFO "%s: Entering power state D0 %s (%d).\n",
-                        dev->name, ret ? "failed" : "succeeded", ret);
+                netdev_info(dev, "Entering power state D0 %s (%d)\n",
+                            ret ? "failed" : "succeeded", ret);
 
         pci_restore_state(pdev);
 
@@ -2028,17 +2316,15 @@ static int __init rhine_init(void)
 {
 /* when a module, this is printed whether or not devices are found in probe */
 #ifdef MODULE
-        printk(version);
+        pr_info("%s\n", version);
 #endif
         if (dmi_check_system(rhine_dmi_table)) {
                 /* these BIOSes fail at PXE boot if chip is in D3 */
                 avoid_D3 = 1;
-                printk(KERN_WARNING "%s: Broken BIOS detected, avoid_D3 "
-                                    "enabled.\n",
-                       DRV_NAME);
+                pr_warn("Broken BIOS detected, avoid_D3 enabled\n");
         }
         else if (avoid_D3)
-                printk(KERN_INFO "%s: avoid_D3 set.\n", DRV_NAME);
+                pr_info("avoid_D3 set\n");
 
         return pci_register_driver(&rhine_driver);
 }