net: stmmac: Implement VLAN Hash Filtering in XGMAC

Implement the VLAN Hash Filtering feature in XGMAC core.

Signed-off-by: Jose Abreu <joabreu@synopsys.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/drivers/net/ethernet/stmicro/stmmac/common.h b/drivers/net/ethernet/stmicro/stmmac/common.h
index 45a997fe571cfd0fafd45b0644ac38790e3ffa85..e1e6f67041ec50d0b8e137271aa91cd678fafeef 100644 (file)
--- a/drivers/net/ethernet/stmicro/stmmac/common.h
+++ b/drivers/net/ethernet/stmicro/stmmac/common.h
@@ -355,6 +355,7 @@ struct dma_features {
        unsigned int frpes;
        unsigned int addr64;
        unsigned int rssen;
+       unsigned int vlhash;
 };
 
 /* GMAC TX FIFO is 8K, Rx FIFO is 16K */

diff --git a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2.h b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2.h
index ed3a85f73a72b8eeaaaf57390bb31ae018179884..bae0b01000ccb7d450257e8e2cde572042dd9326 100644 (file)
--- a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2.h
+++ b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2.h
@@ -44,6 +44,7 @@
 #define XGMAC_CORE_INIT_RX             0
 #define XGMAC_PACKET_FILTER            0x00000008
 #define XGMAC_FILTER_RA                        BIT(31)
+#define XGMAC_FILTER_VTFE              BIT(16)
 #define XGMAC_FILTER_HPF               BIT(10)
 #define XGMAC_FILTER_PCF               BIT(7)
 #define XGMAC_FILTER_PM                        BIT(4)
@@ -51,6 +52,14 @@
 #define XGMAC_FILTER_PR                        BIT(0)
 #define XGMAC_HASH_TABLE(x)            (0x00000010 + (x) * 4)
 #define XGMAC_MAX_HASH_TABLE           8
+#define XGMAC_VLAN_TAG                 0x00000050
+#define XGMAC_VLAN_EDVLP               BIT(26)
+#define XGMAC_VLAN_VTHM                        BIT(25)
+#define XGMAC_VLAN_DOVLTC              BIT(20)
+#define XGMAC_VLAN_ESVL                        BIT(18)
+#define XGMAC_VLAN_ETV                 BIT(16)
+#define XGMAC_VLAN_VID                 GENMASK(15, 0)
+#define XGMAC_VLAN_HASH_TABLE          0x00000058
 #define XGMAC_RXQ_CTRL0                        0x000000a0
 #define XGMAC_RXQEN(x)                 GENMASK((x) * 2 + 1, (x) * 2)
 #define XGMAC_RXQEN_SHIFT(x)           ((x) * 2)
@@ -87,6 +96,7 @@
 #define XGMAC_HWFEAT_MMCSEL            BIT(8)
 #define XGMAC_HWFEAT_MGKSEL            BIT(7)
 #define XGMAC_HWFEAT_RWKSEL            BIT(6)
+#define XGMAC_HWFEAT_VLHASH            BIT(4)
 #define XGMAC_HWFEAT_GMIISEL           BIT(1)
 #define XGMAC_HW_FEATURE1              0x00000120
 #define XGMAC_HWFEAT_RSSEN             BIT(20)

diff --git a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c
index 04eec85acc59a4a2b9dc115991b8275cecdd9c44..e2dbebeb59e9dad29696ba8821521e3835d52a70 100644 (file)
--- a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c
@@ -490,6 +490,46 @@ static int dwxgmac2_rss_configure(struct mac_device_info *hw,
        return 0;
 }
 
+static void dwxgmac2_update_vlan_hash(struct mac_device_info *hw, u32 hash,
+                                     bool is_double)
+{
+       void __iomem *ioaddr = hw->pcsr;
+
+       writel(hash, ioaddr + XGMAC_VLAN_HASH_TABLE);
+
+       if (hash) {
+               u32 value = readl(ioaddr + XGMAC_PACKET_FILTER);
+
+               value |= XGMAC_FILTER_VTFE;
+
+               writel(value, ioaddr + XGMAC_PACKET_FILTER);
+
+               value |= XGMAC_VLAN_VTHM | XGMAC_VLAN_ETV;
+               if (is_double) {
+                       value |= XGMAC_VLAN_EDVLP;
+                       value |= XGMAC_VLAN_ESVL;
+                       value |= XGMAC_VLAN_DOVLTC;
+               }
+
+               writel(value, ioaddr + XGMAC_VLAN_TAG);
+       } else {
+               u32 value = readl(ioaddr + XGMAC_PACKET_FILTER);
+
+               value &= ~XGMAC_FILTER_VTFE;
+
+               writel(value, ioaddr + XGMAC_PACKET_FILTER);
+
+               value = readl(ioaddr + XGMAC_VLAN_TAG);
+
+               value &= ~(XGMAC_VLAN_VTHM | XGMAC_VLAN_ETV);
+               value &= ~(XGMAC_VLAN_EDVLP | XGMAC_VLAN_ESVL);
+               value &= ~XGMAC_VLAN_DOVLTC;
+               value &= ~XGMAC_VLAN_VID;
+
+               writel(value, ioaddr + XGMAC_VLAN_TAG);
+       }
+}
+
 const struct stmmac_ops dwxgmac210_ops = {
        .core_init = dwxgmac2_core_init,
        .set_mac = dwxgmac2_set_mac,
@@ -521,6 +561,7 @@ const struct stmmac_ops dwxgmac210_ops = {
        .set_filter = dwxgmac2_set_filter,
        .set_mac_loopback = dwxgmac2_set_mac_loopback,
        .rss_configure = dwxgmac2_rss_configure,
+       .update_vlan_hash = dwxgmac2_update_vlan_hash,
 };
 
 int dwxgmac2_setup(struct stmmac_priv *priv)

diff --git a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_dma.c b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_dma.c
index 45a6634ee397c6240ee85f32092583c381783045..b50e275e76c2cffdc2091dac8caf914cb742d4ea 100644 (file)
--- a/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_dma.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwxgmac2_dma.c
@@ -359,6 +359,7 @@ static void dwxgmac2_get_hw_feature(void __iomem *ioaddr,
        dma_cap->rmon = (hw_cap & XGMAC_HWFEAT_MMCSEL) >> 8;
        dma_cap->pmt_magic_frame = (hw_cap & XGMAC_HWFEAT_MGKSEL) >> 7;
        dma_cap->pmt_remote_wake_up = (hw_cap & XGMAC_HWFEAT_RWKSEL) >> 6;
+       dma_cap->vlhash = (hw_cap & XGMAC_HWFEAT_VLHASH) >> 4;
        dma_cap->mbps_1000 = (hw_cap & XGMAC_HWFEAT_GMIISEL) >> 1;
 
        /* MAC HW feature 1 */

diff --git a/drivers/net/ethernet/stmicro/stmmac/hwif.h b/drivers/net/ethernet/stmicro/stmmac/hwif.h
index bfe7efee9481618f765fb3151b304c4dc0b51961..52fc2344b066b4f390df09d5d743b8b7e7ba8501 100644 (file)
--- a/drivers/net/ethernet/stmicro/stmmac/hwif.h
+++ b/drivers/net/ethernet/stmicro/stmmac/hwif.h
@@ -336,6 +336,9 @@ struct stmmac_ops {
        /* RSS */
        int (*rss_configure)(struct mac_device_info *hw,
                             struct stmmac_rss *cfg, u32 num_rxq);
+       /* VLAN */
+       void (*update_vlan_hash)(struct mac_device_info *hw, u32 hash,
+                                bool is_double);
 };
 
 #define stmmac_core_init(__priv, __args...) \
@@ -408,6 +411,8 @@ struct stmmac_ops {
        stmmac_do_void_callback(__priv, mac, set_mac_loopback, __args)
 #define stmmac_rss_configure(__priv, __args...) \
        stmmac_do_callback(__priv, mac, rss_configure, __args)
+#define stmmac_update_vlan_hash(__priv, __args...) \
+       stmmac_do_void_callback(__priv, mac, update_vlan_hash, __args)
 
 /* PTP and HW Timer helpers */
 struct stmmac_hwtimestamp {

diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac.h b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
index d2f6f56ae29cb48d9a83571c62047c3dd364678a..4179559b11ade8cb8a7b2278bf525fb489710a61 100644 (file)
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
@@ -13,6 +13,7 @@
 #define DRV_MODULE_VERSION     "Jan_2016"
 
 #include <linux/clk.h>
+#include <linux/if_vlan.h>
 #include <linux/stmmac.h>
 #include <linux/phylink.h>
 #include <linux/pci.h>
@@ -191,6 +192,7 @@ struct stmmac_priv {
        spinlock_t ptp_lock;
        void __iomem *mmcaddr;
        void __iomem *ptpaddr;
+       unsigned long active_vlans[BITS_TO_LONGS(VLAN_N_VID)];
 
 #ifdef CONFIG_DEBUG_FS
        struct dentry *dbgfs_dir;

diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index 404a0548f213619cee2e4def8ba64c0780fd70f6..2274bb58eefac5e7d197551c43e263db30ef8716 100644 (file)
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -4037,6 +4037,79 @@ static void stmmac_exit_fs(struct net_device *dev)
 }
 #endif /* CONFIG_DEBUG_FS */
 
+static u32 stmmac_vid_crc32_le(__le16 vid_le)
+{
+       unsigned char *data = (unsigned char *)&vid_le;
+       unsigned char data_byte = 0;
+       u32 crc = ~0x0;
+       u32 temp = 0;
+       int i, bits;
+
+       bits = get_bitmask_order(VLAN_VID_MASK);
+       for (i = 0; i < bits; i++) {
+               if ((i % 8) == 0)
+                       data_byte = data[i / 8];
+
+               temp = ((crc & 1) ^ data_byte) & 1;
+               crc >>= 1;
+               data_byte >>= 1;
+
+               if (temp)
+                       crc ^= 0xedb88320;
+       }
+
+       return crc;
+}
+
+static int stmmac_vlan_update(struct stmmac_priv *priv, bool is_double)
+{
+       u32 crc, hash = 0;
+       u16 vid;
+
+       for_each_set_bit(vid, priv->active_vlans, VLAN_N_VID) {
+               __le16 vid_le = cpu_to_le16(vid);
+               crc = bitrev32(~stmmac_vid_crc32_le(vid_le)) >> 28;
+               hash |= (1 << crc);
+       }
+
+       return stmmac_update_vlan_hash(priv, priv->hw, hash, is_double);
+}
+
+static int stmmac_vlan_rx_add_vid(struct net_device *ndev, __be16 proto, u16 vid)
+{
+       struct stmmac_priv *priv = netdev_priv(ndev);
+       bool is_double = false;
+       int ret;
+
+       if (!priv->dma_cap.vlhash)
+               return -EOPNOTSUPP;
+       if (be16_to_cpu(proto) == ETH_P_8021AD)
+               is_double = true;
+
+       set_bit(vid, priv->active_vlans);
+       ret = stmmac_vlan_update(priv, is_double);
+       if (ret) {
+               clear_bit(vid, priv->active_vlans);
+               return ret;
+       }
+
+       return ret;
+}
+
+static int stmmac_vlan_rx_kill_vid(struct net_device *ndev, __be16 proto, u16 vid)
+{
+       struct stmmac_priv *priv = netdev_priv(ndev);
+       bool is_double = false;
+
+       if (!priv->dma_cap.vlhash)
+               return -EOPNOTSUPP;
+       if (be16_to_cpu(proto) == ETH_P_8021AD)
+               is_double = true;
+
+       clear_bit(vid, priv->active_vlans);
+       return stmmac_vlan_update(priv, is_double);
+}
+
 static const struct net_device_ops stmmac_netdev_ops = {
        .ndo_open = stmmac_open,
        .ndo_start_xmit = stmmac_xmit,
@@ -4053,6 +4126,8 @@ static const struct net_device_ops stmmac_netdev_ops = {
        .ndo_poll_controller = stmmac_poll_controller,
 #endif
        .ndo_set_mac_address = stmmac_set_mac_address,
+       .ndo_vlan_rx_add_vid = stmmac_vlan_rx_add_vid,
+       .ndo_vlan_rx_kill_vid = stmmac_vlan_rx_kill_vid,
 };
 
 static void stmmac_reset_subtask(struct stmmac_priv *priv)
@@ -4307,6 +4382,10 @@ int stmmac_dvr_probe(struct device *device,
 #ifdef STMMAC_VLAN_TAG_USED
        /* Both mac100 and gmac support receive VLAN tag detection */
        ndev->features |= NETIF_F_HW_VLAN_CTAG_RX | NETIF_F_HW_VLAN_STAG_RX;
+       if (priv->dma_cap.vlhash) {
+               ndev->features |= NETIF_F_HW_VLAN_CTAG_FILTER;
+               ndev->features |= NETIF_F_HW_VLAN_STAG_FILTER;
+       }
 #endif
        priv->msg_enable = netif_msg_init(debug, default_msg_level);