net: stmmac: Add support for VLAN Insertion Offload
authorJose Abreu <Jose.Abreu@synopsys.com>
Sat, 17 Aug 2019 18:54:50 +0000 (20:54 +0200)
committerDavid S. Miller <davem@davemloft.net>
Sat, 17 Aug 2019 19:43:59 +0000 (12:43 -0700)
Adds the logic to insert a given VLAN ID in a packet. This is offloaded
to HW and its descriptor based. For now, only XGMAC implements the
necessary callbacks.

Signed-off-by: Jose Abreu <joabreu@synopsys.com>
Signed-off-by: David S. Miller <davem@davemloft.net>
drivers/net/ethernet/stmicro/stmmac/common.h
drivers/net/ethernet/stmicro/stmmac/dwxgmac2.h
drivers/net/ethernet/stmicro/stmmac/dwxgmac2_core.c
drivers/net/ethernet/stmicro/stmmac/dwxgmac2_descs.c
drivers/net/ethernet/stmicro/stmmac/dwxgmac2_dma.c
drivers/net/ethernet/stmicro/stmmac/hwif.h
drivers/net/ethernet/stmicro/stmmac/stmmac_main.c

index 1303ec81fd3d2d5d8b24ac98d1d031d2f3710485..49aa56ca09cca1a1b3d85d9495ff4967b312f8ff 100644 (file)
@@ -358,6 +358,8 @@ struct dma_features {
        unsigned int rssen;
        unsigned int vlhash;
        unsigned int sphen;
+       unsigned int vlins;
+       unsigned int dvlan;
 };
 
 /* GMAC TX FIFO is 8K, Rx FIFO is 16K */
@@ -389,6 +391,12 @@ struct dma_features {
 #define STMMAC_RSS_HASH_KEY_SIZE       40
 #define STMMAC_RSS_MAX_TABLE_SIZE      256
 
+/* VLAN */
+#define STMMAC_VLAN_NONE       0x0
+#define STMMAC_VLAN_REMOVE     0x1
+#define STMMAC_VLAN_INSERT     0x2
+#define STMMAC_VLAN_REPLACE    0x3
+
 extern const struct stmmac_desc_ops enh_desc_ops;
 extern const struct stmmac_desc_ops ndesc_ops;
 
index 79c145ba25a82ed209560f03159ccfe43a853b8d..7357b8bdc1282b78d28b8e04de70106436d1a835 100644 (file)
 #define XGMAC_VLAN_ETV                 BIT(16)
 #define XGMAC_VLAN_VID                 GENMASK(15, 0)
 #define XGMAC_VLAN_HASH_TABLE          0x00000058
+#define XGMAC_VLAN_INCL                        0x00000060
+#define XGMAC_VLAN_VLTI                        BIT(20)
+#define XGMAC_VLAN_CSVL                        BIT(19)
+#define XGMAC_VLAN_VLC                 GENMASK(17, 16)
+#define XGMAC_VLAN_VLC_SHIFT           16
 #define XGMAC_RXQ_CTRL0                        0x000000a0
 #define XGMAC_RXQEN(x)                 GENMASK((x) * 2 + 1, (x) * 2)
 #define XGMAC_RXQEN_SHIFT(x)           ((x) * 2)
 #define XGMAC_HWFEAT_RXQCNT            GENMASK(3, 0)
 #define XGMAC_HW_FEATURE3              0x00000128
 #define XGMAC_HWFEAT_ASP               GENMASK(15, 14)
+#define XGMAC_HWFEAT_DVLAN             BIT(13)
 #define XGMAC_HWFEAT_FRPES             GENMASK(12, 11)
 #define XGMAC_HWFEAT_FRPPB             GENMASK(10, 9)
 #define XGMAC_HWFEAT_FRPSEL            BIT(3)
 #define XGMAC_REGSIZE                  ((0x0000317c + (0x80 * 15)) / 4)
 
 /* Descriptors */
+#define XGMAC_TDES2_IVT                        GENMASK(31, 16)
+#define XGMAC_TDES2_IVT_SHIFT          16
 #define XGMAC_TDES2_IOC                        BIT(31)
 #define XGMAC_TDES2_TTSE               BIT(30)
 #define XGMAC_TDES2_B2L                        GENMASK(29, 16)
 #define XGMAC_TDES2_B2L_SHIFT          16
+#define XGMAC_TDES2_VTIR               GENMASK(15, 14)
+#define XGMAC_TDES2_VTIR_SHIFT         14
 #define XGMAC_TDES2_B1L                        GENMASK(13, 0)
 #define XGMAC_TDES3_OWN                        BIT(31)
 #define XGMAC_TDES3_CTXT               BIT(30)
 #define XGMAC_TDES3_SAIC_SHIFT         23
 #define XGMAC_TDES3_THL                        GENMASK(22, 19)
 #define XGMAC_TDES3_THL_SHIFT          19
+#define XGMAC_TDES3_IVTIR              GENMASK(19, 18)
+#define XGMAC_TDES3_IVTIR_SHIFT                18
 #define XGMAC_TDES3_TSE                        BIT(18)
+#define XGMAC_TDES3_IVLTV              BIT(17)
 #define XGMAC_TDES3_CIC                        GENMASK(17, 16)
 #define XGMAC_TDES3_CIC_SHIFT          16
 #define XGMAC_TDES3_TPL                        GENMASK(17, 0)
+#define XGMAC_TDES3_VLTV               BIT(16)
+#define XGMAC_TDES3_VT                 GENMASK(15, 0)
 #define XGMAC_TDES3_FL                 GENMASK(14, 0)
 #define XGMAC_RDES2_HL                 GENMASK(9, 0)
 #define XGMAC_RDES3_OWN                        BIT(31)
index d8483d088711ea5237aa092f249f998dfc7ba4dd..e534a3aaf4a31737266495422ce9a1cf9836b368 100644 (file)
@@ -1150,6 +1150,19 @@ static void dwxgmac2_sarc_configure(void __iomem *ioaddr, int val)
        writel(value, ioaddr + XGMAC_TX_CONFIG);
 }
 
+static void dwxgmac2_enable_vlan(struct mac_device_info *hw, u32 type)
+{
+       void __iomem *ioaddr = hw->pcsr;
+       u32 value;
+
+       value = readl(ioaddr + XGMAC_VLAN_INCL);
+       value |= XGMAC_VLAN_VLTI;
+       value |= XGMAC_VLAN_CSVL; /* Only use SVLAN */
+       value &= ~XGMAC_VLAN_VLC;
+       value |= (type << XGMAC_VLAN_VLC_SHIFT) & XGMAC_VLAN_VLC;
+       writel(value, ioaddr + XGMAC_VLAN_INCL);
+}
+
 const struct stmmac_ops dwxgmac210_ops = {
        .core_init = dwxgmac2_core_init,
        .set_mac = dwxgmac2_set_mac,
@@ -1189,6 +1202,7 @@ const struct stmmac_ops dwxgmac210_ops = {
        .get_mac_tx_timestamp = dwxgmac2_get_mac_tx_timestamp,
        .flex_pps_config = dwxgmac2_flex_pps_config,
        .sarc_configure = dwxgmac2_sarc_configure,
+       .enable_vlan = dwxgmac2_enable_vlan,
 };
 
 int dwxgmac2_setup(struct stmmac_priv *priv)
index ab11e73ac6b1675e8f9162bfa78a44ccdbba6954..ae48154f933cb1bd3e192874d77f76866478e566 100644 (file)
@@ -305,6 +305,39 @@ static void dwxgmac2_set_sarc(struct dma_desc *p, u32 sarc_type)
        p->des3 |= cpu_to_le32(sarc_type & XGMAC_TDES3_SAIC);
 }
 
+static void dwxgmac2_set_vlan_tag(struct dma_desc *p, u16 tag, u16 inner_tag,
+                                 u32 inner_type)
+{
+       p->des0 = 0;
+       p->des1 = 0;
+       p->des2 = 0;
+       p->des3 = 0;
+
+       /* Inner VLAN */
+       if (inner_type) {
+               u32 des = inner_tag << XGMAC_TDES2_IVT_SHIFT;
+
+               des &= XGMAC_TDES2_IVT;
+               p->des2 = cpu_to_le32(des);
+
+               des = inner_type << XGMAC_TDES3_IVTIR_SHIFT;
+               des &= XGMAC_TDES3_IVTIR;
+               p->des3 = cpu_to_le32(des | XGMAC_TDES3_IVLTV);
+       }
+
+       /* Outer VLAN */
+       p->des3 |= cpu_to_le32(tag & XGMAC_TDES3_VT);
+       p->des3 |= cpu_to_le32(XGMAC_TDES3_VLTV);
+
+       p->des3 |= cpu_to_le32(XGMAC_TDES3_CTXT);
+}
+
+static void dwxgmac2_set_vlan(struct dma_desc *p, u32 type)
+{
+       type <<= XGMAC_TDES2_VTIR_SHIFT;
+       p->des2 |= cpu_to_le32(type & XGMAC_TDES2_VTIR);
+}
+
 const struct stmmac_desc_ops dwxgmac210_desc_ops = {
        .tx_status = dwxgmac2_get_tx_status,
        .rx_status = dwxgmac2_get_rx_status,
@@ -332,4 +365,6 @@ const struct stmmac_desc_ops dwxgmac210_desc_ops = {
        .get_rx_header_len = dwxgmac2_get_rx_header_len,
        .set_sec_addr = dwxgmac2_set_sec_addr,
        .set_sarc = dwxgmac2_set_sarc,
+       .set_vlan_tag = dwxgmac2_set_vlan_tag,
+       .set_vlan = dwxgmac2_set_vlan,
 };
index f2d5901fbaff519369dc292c3f953f89267feb48..64956465c03019edb0d8f5c2c61f3f61d0f50192 100644 (file)
@@ -359,6 +359,7 @@ static void dwxgmac2_get_hw_feature(void __iomem *ioaddr,
 
        /*  MAC HW feature 0 */
        hw_cap = readl(ioaddr + XGMAC_HW_FEATURE0);
+       dma_cap->vlins = (hw_cap & XGMAC_HWFEAT_SAVLANINS) >> 27;
        dma_cap->rx_coe = (hw_cap & XGMAC_HWFEAT_RXCOESEL) >> 16;
        dma_cap->tx_coe = (hw_cap & XGMAC_HWFEAT_TXCOESEL) >> 14;
        dma_cap->eee = (hw_cap & XGMAC_HWFEAT_EEESEL) >> 13;
@@ -413,6 +414,7 @@ static void dwxgmac2_get_hw_feature(void __iomem *ioaddr,
        /* MAC HW feature 3 */
        hw_cap = readl(ioaddr + XGMAC_HW_FEATURE3);
        dma_cap->asp = (hw_cap & XGMAC_HWFEAT_ASP) >> 14;
+       dma_cap->dvlan = (hw_cap & XGMAC_HWFEAT_DVLAN) >> 13;
        dma_cap->frpes = (hw_cap & XGMAC_HWFEAT_FRPES) >> 11;
        dma_cap->frpbs = (hw_cap & XGMAC_HWFEAT_FRPPB) >> 9;
        dma_cap->frpsel = (hw_cap & XGMAC_HWFEAT_FRPSEL) >> 3;
index e54864cde01b7d7ffa03cb79de9aa59f737f1295..9435b312495db7fc1fcd8dcf30254259823cf960 100644 (file)
@@ -92,6 +92,9 @@ struct stmmac_desc_ops {
        int (*get_rx_header_len)(struct dma_desc *p, unsigned int *len);
        void (*set_sec_addr)(struct dma_desc *p, dma_addr_t addr);
        void (*set_sarc)(struct dma_desc *p, u32 sarc_type);
+       void (*set_vlan_tag)(struct dma_desc *p, u16 tag, u16 inner_tag,
+                            u32 inner_type);
+       void (*set_vlan)(struct dma_desc *p, u32 type);
 };
 
 #define stmmac_init_rx_desc(__priv, __args...) \
@@ -150,6 +153,10 @@ struct stmmac_desc_ops {
        stmmac_do_void_callback(__priv, desc, set_sec_addr, __args)
 #define stmmac_set_desc_sarc(__priv, __args...) \
        stmmac_do_void_callback(__priv, desc, set_sarc, __args)
+#define stmmac_set_desc_vlan_tag(__priv, __args...) \
+       stmmac_do_void_callback(__priv, desc, set_vlan_tag, __args)
+#define stmmac_set_desc_vlan(__priv, __args...) \
+       stmmac_do_void_callback(__priv, desc, set_vlan, __args)
 
 struct stmmac_dma_cfg;
 struct dma_features;
@@ -351,6 +358,7 @@ struct stmmac_ops {
        /* VLAN */
        void (*update_vlan_hash)(struct mac_device_info *hw, u32 hash,
                                 bool is_double);
+       void (*enable_vlan)(struct mac_device_info *hw, u32 type);
        /* TX Timestamp */
        int (*get_mac_tx_timestamp)(struct mac_device_info *hw, u64 *ts);
        /* Source Address Insertion / Replacement */
@@ -429,6 +437,8 @@ struct stmmac_ops {
        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)
+#define stmmac_enable_vlan(__priv, __args...) \
+       stmmac_do_void_callback(__priv, mac, enable_vlan, __args)
 #define stmmac_get_mac_tx_timestamp(__priv, __args...) \
        stmmac_do_callback(__priv, mac, get_mac_tx_timestamp, __args)
 #define stmmac_sarc_configure(__priv, __args...) \
index 8e38b053d9ab4c74dd06d1f35eed044dfdfefb9b..bd1078433448c8cf5dc770c2c53563d5183d2d9f 100644 (file)
@@ -2617,6 +2617,10 @@ static int stmmac_hw_setup(struct net_device *dev, bool init_ptp)
                        stmmac_enable_sph(priv, priv->ioaddr, 1, chan);
        }
 
+       /* VLAN Tag Insertion */
+       if (priv->dma_cap.vlins)
+               stmmac_enable_vlan(priv, priv->hw, STMMAC_VLAN_INSERT);
+
        /* Start the ball rolling... */
        stmmac_start_all_dma(priv);
 
@@ -2794,6 +2798,33 @@ static int stmmac_release(struct net_device *dev)
        return 0;
 }
 
+static bool stmmac_vlan_insert(struct stmmac_priv *priv, struct sk_buff *skb,
+                              struct stmmac_tx_queue *tx_q)
+{
+       u16 tag = 0x0, inner_tag = 0x0;
+       u32 inner_type = 0x0;
+       struct dma_desc *p;
+
+       if (!priv->dma_cap.vlins)
+               return false;
+       if (!skb_vlan_tag_present(skb))
+               return false;
+       if (skb->vlan_proto == htons(ETH_P_8021AD)) {
+               inner_tag = skb_vlan_tag_get(skb);
+               inner_type = STMMAC_VLAN_INSERT;
+       }
+
+       tag = skb_vlan_tag_get(skb);
+
+       p = tx_q->dma_tx + tx_q->cur_tx;
+       if (stmmac_set_desc_vlan_tag(priv, p, tag, inner_tag, inner_type))
+               return false;
+
+       stmmac_set_tx_owner(priv, p);
+       tx_q->cur_tx = STMMAC_GET_ENTRY(tx_q->cur_tx, DMA_TX_SIZE);
+       return true;
+}
+
 /**
  *  stmmac_tso_allocator - close entry point of the driver
  *  @priv: driver private structure
@@ -2873,12 +2904,13 @@ static netdev_tx_t stmmac_tso_xmit(struct sk_buff *skb, struct net_device *dev)
        struct stmmac_priv *priv = netdev_priv(dev);
        int nfrags = skb_shinfo(skb)->nr_frags;
        u32 queue = skb_get_queue_mapping(skb);
-       unsigned int first_entry;
        struct stmmac_tx_queue *tx_q;
+       unsigned int first_entry;
        int tmp_pay_len = 0;
        u32 pay_len, mss;
        u8 proto_hdr_len;
        dma_addr_t des;
+       bool has_vlan;
        int i;
 
        tx_q = &priv->tx_queue[queue];
@@ -2920,12 +2952,18 @@ static netdev_tx_t stmmac_tso_xmit(struct sk_buff *skb, struct net_device *dev)
                        skb->data_len);
        }
 
+       /* Check if VLAN can be inserted by HW */
+       has_vlan = stmmac_vlan_insert(priv, skb, tx_q);
+
        first_entry = tx_q->cur_tx;
        WARN_ON(tx_q->tx_skbuff[first_entry]);
 
        desc = tx_q->dma_tx + first_entry;
        first = desc;
 
+       if (has_vlan)
+               stmmac_set_desc_vlan(priv, first, STMMAC_VLAN_INSERT);
+
        /* first descriptor: fill Headers on Buf1 */
        des = dma_map_single(priv->device, skb->data, skb_headlen(skb),
                             DMA_TO_DEVICE);
@@ -3085,6 +3123,7 @@ static netdev_tx_t stmmac_xmit(struct sk_buff *skb, struct net_device *dev)
        unsigned int first_entry;
        unsigned int enh_desc;
        dma_addr_t des;
+       bool has_vlan;
        int entry;
 
        tx_q = &priv->tx_queue[queue];
@@ -3110,6 +3149,9 @@ static netdev_tx_t stmmac_xmit(struct sk_buff *skb, struct net_device *dev)
                return NETDEV_TX_BUSY;
        }
 
+       /* Check if VLAN can be inserted by HW */
+       has_vlan = stmmac_vlan_insert(priv, skb, tx_q);
+
        entry = tx_q->cur_tx;
        first_entry = entry;
        WARN_ON(tx_q->tx_skbuff[first_entry]);
@@ -3123,6 +3165,9 @@ static netdev_tx_t stmmac_xmit(struct sk_buff *skb, struct net_device *dev)
 
        first = desc;
 
+       if (has_vlan)
+               stmmac_set_desc_vlan(priv, first, STMMAC_VLAN_INSERT);
+
        enh_desc = priv->plat->enh_desc;
        /* To program the descriptors according to the size of the frame */
        if (enh_desc)
@@ -4473,6 +4518,11 @@ int stmmac_dvr_probe(struct device *device,
                ndev->features |= NETIF_F_HW_VLAN_CTAG_FILTER;
                ndev->features |= NETIF_F_HW_VLAN_STAG_FILTER;
        }
+       if (priv->dma_cap.vlins) {
+               ndev->features |= NETIF_F_HW_VLAN_CTAG_TX;
+               if (priv->dma_cap.dvlan)
+                       ndev->features |= NETIF_F_HW_VLAN_STAG_TX;
+       }
 #endif
        priv->msg_enable = netif_msg_init(debug, default_msg_level);