Merge ra.kernel.org:/pub/scm/linux/kernel/git/netdev/net
[linux-2.6-block.git] / drivers / net / ethernet / stmicro / stmmac / stmmac_main.c
index b19ab09cb18f7f78e32db20ec762befa94b4eea9..a6cb2aa60e6485d4a0126d3330ac0c348a9c2baa 100644 (file)
@@ -105,7 +105,7 @@ MODULE_PARM_DESC(chain_mode, "To use chain instead of ring mode");
 static irqreturn_t stmmac_interrupt(int irq, void *dev_id);
 
 #ifdef CONFIG_DEBUG_FS
-static int stmmac_init_fs(struct net_device *dev);
+static void stmmac_init_fs(struct net_device *dev);
 static void stmmac_exit_fs(struct net_device *dev);
 #endif
 
@@ -432,6 +432,7 @@ static void stmmac_get_tx_hwtstamp(struct stmmac_priv *priv,
                                   struct dma_desc *p, struct sk_buff *skb)
 {
        struct skb_shared_hwtstamps shhwtstamp;
+       bool found = false;
        u64 ns = 0;
 
        if (!priv->hwts_tx_en)
@@ -443,9 +444,13 @@ static void stmmac_get_tx_hwtstamp(struct stmmac_priv *priv,
 
        /* check tx tstamp status */
        if (stmmac_get_tx_timestamp_status(priv, p)) {
-               /* get the valid tstamp */
                stmmac_get_timestamp(priv, p, priv->adv_ts, &ns);
+               found = true;
+       } else if (!stmmac_get_mac_tx_timestamp(priv, priv->hw, &ns)) {
+               found = true;
+       }
 
+       if (found) {
                memset(&shhwtstamp, 0, sizeof(struct skb_shared_hwtstamps));
                shhwtstamp.hwtstamp = ns_to_ktime(ns);
 
@@ -453,8 +458,6 @@ static void stmmac_get_tx_hwtstamp(struct stmmac_priv *priv,
                /* pass tstamp to stack */
                skb_tstamp_tx(skb, &shhwtstamp);
        }
-
-       return;
 }
 
 /* stmmac_get_rx_hwtstamp - get HW RX timestamps
@@ -828,15 +831,22 @@ static void stmmac_validate(struct phylink_config *config,
                phylink_set(mask, 1000baseT_Full);
                phylink_set(mask, 1000baseX_Full);
        } else if (priv->plat->has_xgmac) {
-               phylink_set(mac_supported, 2500baseT_Full);
-               phylink_set(mac_supported, 5000baseT_Full);
-               phylink_set(mac_supported, 10000baseSR_Full);
-               phylink_set(mac_supported, 10000baseLR_Full);
-               phylink_set(mac_supported, 10000baseER_Full);
-               phylink_set(mac_supported, 10000baseLRM_Full);
-               phylink_set(mac_supported, 10000baseT_Full);
-               phylink_set(mac_supported, 10000baseKX4_Full);
-               phylink_set(mac_supported, 10000baseKR_Full);
+               if (!max_speed || (max_speed >= 2500)) {
+                       phylink_set(mac_supported, 2500baseT_Full);
+                       phylink_set(mac_supported, 2500baseX_Full);
+               }
+               if (!max_speed || (max_speed >= 5000)) {
+                       phylink_set(mac_supported, 5000baseT_Full);
+               }
+               if (!max_speed || (max_speed >= 10000)) {
+                       phylink_set(mac_supported, 10000baseSR_Full);
+                       phylink_set(mac_supported, 10000baseLR_Full);
+                       phylink_set(mac_supported, 10000baseER_Full);
+                       phylink_set(mac_supported, 10000baseLRM_Full);
+                       phylink_set(mac_supported, 10000baseT_Full);
+                       phylink_set(mac_supported, 10000baseKX4_Full);
+                       phylink_set(mac_supported, 10000baseKR_Full);
+               }
        }
 
        /* Half-Duplex can only work with single queue */
@@ -1026,7 +1036,7 @@ static int stmmac_init_phy(struct net_device *dev)
 static int stmmac_phy_setup(struct stmmac_priv *priv)
 {
        struct fwnode_handle *fwnode = of_fwnode_handle(priv->plat->phylink_node);
-       int mode = priv->plat->interface;
+       int mode = priv->plat->phy_interface;
        struct phylink *phylink;
 
        priv->phylink_config.dev = &priv->dev->dev;
@@ -1198,6 +1208,17 @@ static int stmmac_init_rx_buffers(struct stmmac_priv *priv, struct dma_desc *p,
        if (!buf->page)
                return -ENOMEM;
 
+       if (priv->sph) {
+               buf->sec_page = page_pool_dev_alloc_pages(rx_q->page_pool);
+               if (!buf->sec_page)
+                       return -ENOMEM;
+
+               buf->sec_addr = page_pool_get_dma_addr(buf->sec_page);
+               stmmac_set_desc_sec_addr(priv, p, buf->sec_addr);
+       } else {
+               buf->sec_page = NULL;
+       }
+
        buf->addr = page_pool_get_dma_addr(buf->page);
        stmmac_set_desc_addr(priv, p, buf->addr);
        if (priv->dma_buf_sz == BUF_SIZE_16KiB)
@@ -1220,6 +1241,10 @@ static void stmmac_free_rx_buffer(struct stmmac_priv *priv, u32 queue, int i)
        if (buf->page)
                page_pool_put_page(rx_q->page_pool, buf->page, false);
        buf->page = NULL;
+
+       if (buf->sec_page)
+               page_pool_put_page(rx_q->page_pool, buf->sec_page, false);
+       buf->sec_page = NULL;
 }
 
 /**
@@ -2417,6 +2442,22 @@ static void stmmac_mac_config_rx_queues_routing(struct stmmac_priv *priv)
        }
 }
 
+static void stmmac_mac_config_rss(struct stmmac_priv *priv)
+{
+       if (!priv->dma_cap.rssen || !priv->plat->rss_en) {
+               priv->rss.enable = false;
+               return;
+       }
+
+       if (priv->dev->features & NETIF_F_RXHASH)
+               priv->rss.enable = true;
+       else
+               priv->rss.enable = false;
+
+       stmmac_rss_configure(priv, priv->hw, &priv->rss,
+                            priv->plat->rx_queues_to_use);
+}
+
 /**
  *  stmmac_mtl_configuration - Configure MTL
  *  @priv: driver private structure
@@ -2461,6 +2502,10 @@ static void stmmac_mtl_configuration(struct stmmac_priv *priv)
        /* Set RX routing */
        if (rx_queues_count > 1)
                stmmac_mac_config_rx_queues_routing(priv);
+
+       /* Receive Side Scaling */
+       if (rx_queues_count > 1)
+               stmmac_mac_config_rss(priv);
 }
 
 static void stmmac_safety_feat_configuration(struct stmmac_priv *priv)
@@ -2573,6 +2618,16 @@ static int stmmac_hw_setup(struct net_device *dev, bool init_ptp)
                        stmmac_enable_tso(priv, priv->ioaddr, 1, chan);
        }
 
+       /* Enable Split Header */
+       if (priv->sph && priv->hw->rx_csum) {
+               for (chan = 0; chan < rx_cnt; chan++)
+                       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);
 
@@ -2750,6 +2805,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
@@ -2829,12 +2911,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];
@@ -2876,12 +2959,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);
@@ -2960,6 +3049,9 @@ static netdev_tx_t stmmac_tso_xmit(struct sk_buff *skb, struct net_device *dev)
                priv->xstats.tx_set_ic_bit++;
        }
 
+       if (priv->sarc_type)
+               stmmac_set_desc_sarc(priv, first, priv->sarc_type);
+
        skb_tx_timestamp(skb);
 
        if (unlikely((skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) &&
@@ -3038,6 +3130,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];
@@ -3063,6 +3156,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]);
@@ -3076,6 +3172,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)
@@ -3173,6 +3272,9 @@ static netdev_tx_t stmmac_xmit(struct sk_buff *skb, struct net_device *dev)
                priv->xstats.tx_set_ic_bit++;
        }
 
+       if (priv->sarc_type)
+               stmmac_set_desc_sarc(priv, first, priv->sarc_type);
+
        skb_tx_timestamp(skb);
 
        /* Ready to fill the first descriptor and set the OWN bit w/o any
@@ -3292,6 +3394,17 @@ static inline void stmmac_rx_refill(struct stmmac_priv *priv, u32 queue)
                                break;
                }
 
+               if (priv->sph && !buf->sec_page) {
+                       buf->sec_page = page_pool_dev_alloc_pages(rx_q->page_pool);
+                       if (!buf->sec_page)
+                               break;
+
+                       buf->sec_addr = page_pool_get_dma_addr(buf->sec_page);
+
+                       dma_sync_single_for_device(priv->device, buf->sec_addr,
+                                                  len, DMA_FROM_DEVICE);
+               }
+
                buf->addr = page_pool_get_dma_addr(buf->page);
 
                /* Sync whole allocation to device. This will invalidate old
@@ -3301,10 +3414,13 @@ static inline void stmmac_rx_refill(struct stmmac_priv *priv, u32 queue)
                                           DMA_FROM_DEVICE);
 
                stmmac_set_desc_addr(priv, p, buf->addr);
+               stmmac_set_desc_sec_addr(priv, p, buf->sec_addr);
                stmmac_refill_desc3(priv, rx_q, p);
 
                rx_q->rx_count_frames++;
-               rx_q->rx_count_frames %= priv->rx_coal_frames;
+               rx_q->rx_count_frames += priv->rx_coal_frames;
+               if (rx_q->rx_count_frames > priv->rx_coal_frames)
+                       rx_q->rx_count_frames = 0;
                use_rx_wd = priv->use_riwt && rx_q->rx_count_frames;
 
                dma_wmb();
@@ -3330,9 +3446,10 @@ static int stmmac_rx(struct stmmac_priv *priv, int limit, u32 queue)
 {
        struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue];
        struct stmmac_channel *ch = &priv->channel[queue];
+       unsigned int count = 0, error = 0, len = 0;
+       int status = 0, coe = priv->hw->rx_csum;
        unsigned int next_entry = rx_q->cur_rx;
-       int coe = priv->hw->rx_csum;
-       unsigned int count = 0;
+       struct sk_buff *skb = NULL;
 
        if (netif_msg_rx_status(priv)) {
                void *rx_head;
@@ -3346,10 +3463,30 @@ static int stmmac_rx(struct stmmac_priv *priv, int limit, u32 queue)
                stmmac_display_ring(priv, rx_head, DMA_RX_SIZE, true);
        }
        while (count < limit) {
+               unsigned int hlen = 0, prev_len = 0;
+               enum pkt_hash_types hash_type;
                struct stmmac_rx_buffer *buf;
                struct dma_desc *np, *p;
-               int entry, status;
+               unsigned int sec_len;
+               int entry;
+               u32 hash;
+
+               if (!count && rx_q->state_saved) {
+                       skb = rx_q->state.skb;
+                       error = rx_q->state.error;
+                       len = rx_q->state.len;
+               } else {
+                       rx_q->state_saved = false;
+                       skb = NULL;
+                       error = 0;
+                       len = 0;
+               }
 
+               if (count >= limit)
+                       break;
+
+read_again:
+               sec_len = 0;
                entry = next_entry;
                buf = &rx_q->buf_pool[entry];
 
@@ -3376,34 +3513,33 @@ static int stmmac_rx(struct stmmac_priv *priv, int limit, u32 queue)
                        np = rx_q->dma_rx + next_entry;
 
                prefetch(np);
+               prefetch(page_address(buf->page));
 
                if (priv->extend_desc)
                        stmmac_rx_extended_status(priv, &priv->dev->stats,
                                        &priv->xstats, rx_q->dma_erx + entry);
                if (unlikely(status == discard_frame)) {
                        page_pool_recycle_direct(rx_q->page_pool, buf->page);
-                       priv->dev->stats.rx_errors++;
                        buf->page = NULL;
-               } else {
-                       struct sk_buff *skb;
-                       int frame_len;
-                       unsigned int des;
+                       error = 1;
+                       if (!priv->hwts_rx_en)
+                               priv->dev->stats.rx_errors++;
+               }
 
-                       stmmac_get_desc_addr(priv, p, &des);
-                       frame_len = stmmac_get_rx_frame_len(priv, p, coe);
+               if (unlikely(error && (status & rx_not_ls)))
+                       goto read_again;
+               if (unlikely(error)) {
+                       dev_kfree_skb(skb);
+                       continue;
+               }
 
-                       /*  If frame length is greater than skb buffer size
-                        *  (preallocated during init) then the packet is
-                        *  ignored
-                        */
-                       if (frame_len > priv->dma_buf_sz) {
-                               if (net_ratelimit())
-                                       netdev_err(priv->dev,
-                                                  "len %d larger than size (%d)\n",
-                                                  frame_len, priv->dma_buf_sz);
-                               priv->dev->stats.rx_length_errors++;
-                               continue;
-                       }
+               /* Buffer is good. Go on. */
+
+               if (likely(status & rx_not_ls)) {
+                       len += priv->dma_buf_sz;
+               } else {
+                       prev_len = len;
+                       len = stmmac_get_rx_frame_len(priv, p, coe);
 
                        /* ACS is set; GMAC core strips PAD/FCS for IEEE 802.3
                         * Type frames (LLC/LLC-SNAP)
@@ -3414,53 +3550,97 @@ static int stmmac_rx(struct stmmac_priv *priv, int limit, u32 queue)
                         */
                        if (unlikely(priv->synopsys_id >= DWMAC_CORE_4_00) ||
                            unlikely(status != llc_snap))
-                               frame_len -= ETH_FCS_LEN;
+                               len -= ETH_FCS_LEN;
+               }
+
+               if (!skb) {
+                       int ret = stmmac_get_rx_header_len(priv, p, &hlen);
 
-                       if (netif_msg_rx_status(priv)) {
-                               netdev_dbg(priv->dev, "\tdesc: %p [entry %d] buff=0x%x\n",
-                                          p, entry, des);
-                               netdev_dbg(priv->dev, "frame size %d, COE: %d\n",
-                                          frame_len, status);
+                       if (priv->sph && !ret && (hlen > 0)) {
+                               sec_len = len;
+                               if (!(status & rx_not_ls))
+                                       sec_len = sec_len - hlen;
+                               len = hlen;
+
+                               prefetch(page_address(buf->sec_page));
+                               priv->xstats.rx_split_hdr_pkt_n++;
                        }
 
-                       skb = netdev_alloc_skb_ip_align(priv->dev, frame_len);
-                       if (unlikely(!skb)) {
+                       skb = napi_alloc_skb(&ch->rx_napi, len);
+                       if (!skb) {
                                priv->dev->stats.rx_dropped++;
                                continue;
                        }
 
-                       dma_sync_single_for_cpu(priv->device, buf->addr,
-                                               frame_len, DMA_FROM_DEVICE);
+                       dma_sync_single_for_cpu(priv->device, buf->addr, len,
+                                               DMA_FROM_DEVICE);
                        skb_copy_to_linear_data(skb, page_address(buf->page),
-                                               frame_len);
-                       skb_put(skb, frame_len);
+                                               len);
+                       skb_put(skb, len);
 
-                       if (netif_msg_pktdata(priv)) {
-                               netdev_dbg(priv->dev, "frame received (%dbytes)",
-                                          frame_len);
-                               print_pkt(skb->data, frame_len);
-                       }
-
-                       stmmac_get_rx_hwtstamp(priv, p, np, skb);
+                       /* Data payload copied into SKB, page ready for recycle */
+                       page_pool_recycle_direct(rx_q->page_pool, buf->page);
+                       buf->page = NULL;
+               } else {
+                       unsigned int buf_len = len - prev_len;
 
-                       stmmac_rx_vlan(priv->dev, skb);
+                       if (likely(status & rx_not_ls))
+                               buf_len = priv->dma_buf_sz;
 
-                       skb->protocol = eth_type_trans(skb, priv->dev);
+                       dma_sync_single_for_cpu(priv->device, buf->addr,
+                                               buf_len, DMA_FROM_DEVICE);
+                       skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags,
+                                       buf->page, 0, buf_len,
+                                       priv->dma_buf_sz);
 
-                       if (unlikely(!coe))
-                               skb_checksum_none_assert(skb);
-                       else
-                               skb->ip_summed = CHECKSUM_UNNECESSARY;
+                       /* Data payload appended into SKB */
+                       page_pool_release_page(rx_q->page_pool, buf->page);
+                       buf->page = NULL;
+               }
 
-                       napi_gro_receive(&ch->rx_napi, skb);
+               if (sec_len > 0) {
+                       dma_sync_single_for_cpu(priv->device, buf->sec_addr,
+                                               sec_len, DMA_FROM_DEVICE);
+                       skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags,
+                                       buf->sec_page, 0, sec_len,
+                                       priv->dma_buf_sz);
 
-                       /* Data payload copied into SKB, page ready for recycle */
-                       page_pool_recycle_direct(rx_q->page_pool, buf->page);
-                       buf->page = NULL;
+                       len += sec_len;
 
-                       priv->dev->stats.rx_packets++;
-                       priv->dev->stats.rx_bytes += frame_len;
+                       /* Data payload appended into SKB */
+                       page_pool_release_page(rx_q->page_pool, buf->sec_page);
+                       buf->sec_page = NULL;
                }
+
+               if (likely(status & rx_not_ls))
+                       goto read_again;
+
+               /* Got entire packet into SKB. Finish it. */
+
+               stmmac_get_rx_hwtstamp(priv, p, np, skb);
+               stmmac_rx_vlan(priv->dev, skb);
+               skb->protocol = eth_type_trans(skb, priv->dev);
+
+               if (unlikely(!coe))
+                       skb_checksum_none_assert(skb);
+               else
+                       skb->ip_summed = CHECKSUM_UNNECESSARY;
+
+               if (!stmmac_get_rx_hash(priv, p, &hash, &hash_type))
+                       skb_set_hash(skb, hash, hash_type);
+
+               skb_record_rx_queue(skb, queue);
+               napi_gro_receive(&ch->rx_napi, skb);
+
+               priv->dev->stats.rx_packets++;
+               priv->dev->stats.rx_bytes += len;
+       }
+
+       if (status & rx_not_ls) {
+               rx_q->state_saved = true;
+               rx_q->state.skb = skb;
+               rx_q->state.error = error;
+               rx_q->state.len = len;
        }
 
        stmmac_rx_refill(priv, queue);
@@ -3606,6 +3786,8 @@ static int stmmac_set_features(struct net_device *netdev,
                               netdev_features_t features)
 {
        struct stmmac_priv *priv = netdev_priv(netdev);
+       bool sph_en;
+       u32 chan;
 
        /* Keep the COE Type in case of csum is supporting */
        if (features & NETIF_F_RXCSUM)
@@ -3617,6 +3799,10 @@ static int stmmac_set_features(struct net_device *netdev,
         */
        stmmac_rx_ipc(priv, priv->hw);
 
+       sph_en = (priv->hw->rx_csum > 0) && priv->sph;
+       for (chan = 0; chan < priv->plat->rx_queues_to_use; chan++)
+               stmmac_enable_sph(priv, priv->ioaddr, sph_en, chan);
+
        return 0;
 }
 
@@ -3755,12 +3941,17 @@ static int stmmac_setup_tc_block_cb(enum tc_setup_type type, void *type_data,
        struct stmmac_priv *priv = cb_priv;
        int ret = -EOPNOTSUPP;
 
+       if (!tc_cls_can_offload_and_chain0(priv->dev, type_data))
+               return ret;
+
        stmmac_disable_all_queues(priv);
 
        switch (type) {
        case TC_SETUP_CLSU32:
-               if (tc_cls_can_offload_and_chain0(priv->dev, type_data))
-                       ret = stmmac_tc_setup_cls_u32(priv, priv, type_data);
+               ret = stmmac_tc_setup_cls_u32(priv, priv, type_data);
+               break;
+       case TC_SETUP_CLSFLOWER:
+               ret = stmmac_tc_setup_cls(priv, priv, type_data);
                break;
        default:
                break;
@@ -3962,54 +4153,102 @@ static int stmmac_dma_cap_show(struct seq_file *seq, void *v)
 }
 DEFINE_SHOW_ATTRIBUTE(stmmac_dma_cap);
 
-static int stmmac_init_fs(struct net_device *dev)
+static void stmmac_init_fs(struct net_device *dev)
 {
        struct stmmac_priv *priv = netdev_priv(dev);
 
        /* Create per netdev entries */
        priv->dbgfs_dir = debugfs_create_dir(dev->name, stmmac_fs_dir);
 
-       if (!priv->dbgfs_dir || IS_ERR(priv->dbgfs_dir)) {
-               netdev_err(priv->dev, "ERROR failed to create debugfs directory\n");
+       /* Entry to report DMA RX/TX rings */
+       debugfs_create_file("descriptors_status", 0444, priv->dbgfs_dir, dev,
+                           &stmmac_rings_status_fops);
 
-               return -ENOMEM;
+       /* Entry to report the DMA HW features */
+       debugfs_create_file("dma_cap", 0444, priv->dbgfs_dir, dev,
+                           &stmmac_dma_cap_fops);
+}
+
+static void stmmac_exit_fs(struct net_device *dev)
+{
+       struct stmmac_priv *priv = netdev_priv(dev);
+
+       debugfs_remove_recursive(priv->dbgfs_dir);
+}
+#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;
        }
 
-       /* Entry to report DMA RX/TX rings */
-       priv->dbgfs_rings_status =
-               debugfs_create_file("descriptors_status", 0444,
-                                   priv->dbgfs_dir, dev,
-                                   &stmmac_rings_status_fops);
+       return crc;
+}
 
-       if (!priv->dbgfs_rings_status || IS_ERR(priv->dbgfs_rings_status)) {
-               netdev_err(priv->dev, "ERROR creating stmmac ring debugfs file\n");
-               debugfs_remove_recursive(priv->dbgfs_dir);
+static int stmmac_vlan_update(struct stmmac_priv *priv, bool is_double)
+{
+       u32 crc, hash = 0;
+       u16 vid;
 
-               return -ENOMEM;
+       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);
        }
 
-       /* Entry to report the DMA HW features */
-       priv->dbgfs_dma_cap = debugfs_create_file("dma_cap", 0444,
-                                                 priv->dbgfs_dir,
-                                                 dev, &stmmac_dma_cap_fops);
+       return stmmac_update_vlan_hash(priv, priv->hw, hash, is_double);
+}
 
-       if (!priv->dbgfs_dma_cap || IS_ERR(priv->dbgfs_dma_cap)) {
-               netdev_err(priv->dev, "ERROR creating stmmac MMC debugfs file\n");
-               debugfs_remove_recursive(priv->dbgfs_dir);
+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;
 
-               return -ENOMEM;
+       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 0;
+       return ret;
 }
 
-static void stmmac_exit_fs(struct net_device *dev)
+static int stmmac_vlan_rx_kill_vid(struct net_device *ndev, __be16 proto, u16 vid)
 {
-       struct stmmac_priv *priv = netdev_priv(dev);
+       struct stmmac_priv *priv = netdev_priv(ndev);
+       bool is_double = false;
 
-       debugfs_remove_recursive(priv->dbgfs_dir);
+       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);
 }
-#endif /* CONFIG_DEBUG_FS */
 
 static const struct net_device_ops stmmac_netdev_ops = {
        .ndo_open = stmmac_open,
@@ -4027,6 +4266,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)
@@ -4175,8 +4416,8 @@ int stmmac_dvr_probe(struct device *device,
 {
        struct net_device *ndev = NULL;
        struct stmmac_priv *priv;
-       u32 queue, maxq;
-       int ret = 0;
+       u32 queue, rxq, maxq;
+       int i, ret = 0;
 
        ndev = devm_alloc_etherdev_mqs(device, sizeof(struct stmmac_priv),
                                       MTL_MAX_TX_QUEUES, MTL_MAX_RX_QUEUES);
@@ -4259,6 +4500,12 @@ int stmmac_dvr_probe(struct device *device,
                dev_info(priv->device, "TSO feature enabled\n");
        }
 
+       if (priv->dma_cap.sphen) {
+               ndev->hw_features |= NETIF_F_GRO;
+               priv->sph = true;
+               dev_info(priv->device, "SPH feature enabled\n");
+       }
+
        if (priv->dma_cap.addr64) {
                ret = dma_set_mask_and_coherent(device,
                                DMA_BIT_MASK(priv->dma_cap.addr64));
@@ -4281,15 +4528,33 @@ 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;
+       }
+       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);
 
+       /* Initialize RSS */
+       rxq = priv->plat->rx_queues_to_use;
+       netdev_rss_key_fill(priv->rss.key, sizeof(priv->rss.key));
+       for (i = 0; i < ARRAY_SIZE(priv->rss.table); i++)
+               priv->rss.table[i] = ethtool_rxfh_indir_default(i, rxq);
+
+       if (priv->dma_cap.rssen && priv->plat->rss_en)
+               ndev->features |= NETIF_F_RXHASH;
+
        /* MTU range: 46 - hw-specific max */
        ndev->min_mtu = ETH_ZLEN - ETH_HLEN;
-       if ((priv->plat->enh_desc) || (priv->synopsys_id >= DWMAC_CORE_4_00))
-               ndev->max_mtu = JUMBO_LEN;
-       else if (priv->plat->has_xgmac)
+       if (priv->plat->has_xgmac)
                ndev->max_mtu = XGMAC_JUMBO_LEN;
+       else if ((priv->plat->enh_desc) || (priv->synopsys_id >= DWMAC_CORE_4_00))
+               ndev->max_mtu = JUMBO_LEN;
        else
                ndev->max_mtu = SKB_MAX_HEAD(NET_SKB_PAD + NET_IP_ALIGN);
        /* Will not overwrite ndev->max_mtu if plat->maxmtu > ndev->max_mtu
@@ -4368,10 +4633,7 @@ int stmmac_dvr_probe(struct device *device,
        }
 
 #ifdef CONFIG_DEBUG_FS
-       ret = stmmac_init_fs(ndev);
-       if (ret < 0)
-               netdev_warn(priv->dev, "%s: failed debugFS registration\n",
-                           __func__);
+       stmmac_init_fs(ndev);
 #endif
 
        return ret;
@@ -4621,16 +4883,8 @@ static int __init stmmac_init(void)
 {
 #ifdef CONFIG_DEBUG_FS
        /* Create debugfs main directory if it doesn't exist yet */
-       if (!stmmac_fs_dir) {
+       if (!stmmac_fs_dir)
                stmmac_fs_dir = debugfs_create_dir(STMMAC_RESOURCE_NAME, NULL);
-
-               if (!stmmac_fs_dir || IS_ERR(stmmac_fs_dir)) {
-                       pr_err("ERROR %s, debugfs create directory failed\n",
-                              STMMAC_RESOURCE_NAME);
-
-                       return -ENOMEM;
-               }
-       }
 #endif
 
        return 0;