diff options
| -rw-r--r-- | drivers/net/ethernet/stmicro/stmmac/stmmac_main.c | 21 |
1 files changed, 13 insertions, 8 deletions
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c index 11fb7c777d89..5e6d4fe2f4ef 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c @@ -3182,17 +3182,22 @@ dma_map_err: static void stmmac_rx_vlan(struct net_device *dev, struct sk_buff *skb) { - struct ethhdr *ehdr; + struct vlan_ethhdr *veth; + __be16 vlan_proto; u16 vlanid; - if ((dev->features & NETIF_F_HW_VLAN_CTAG_RX) == - NETIF_F_HW_VLAN_CTAG_RX && - !__vlan_get_tag(skb, &vlanid)) { + veth = (struct vlan_ethhdr *)skb->data; + vlan_proto = veth->h_vlan_proto; + + if ((vlan_proto == htons(ETH_P_8021Q) && + dev->features & NETIF_F_HW_VLAN_CTAG_RX) || + (vlan_proto == htons(ETH_P_8021AD) && + dev->features & NETIF_F_HW_VLAN_STAG_RX)) { /* pop the vlan tag */ - ehdr = (struct ethhdr *)skb->data; - memmove(skb->data + VLAN_HLEN, ehdr, ETH_ALEN * 2); + vlanid = ntohs(veth->h_vlan_TCI); + memmove(skb->data + VLAN_HLEN, veth, ETH_ALEN * 2); skb_pull(skb, VLAN_HLEN); - __vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vlanid); + __vlan_hwaccel_put_tag(skb, vlan_proto, vlanid); } } @@ -4235,7 +4240,7 @@ int stmmac_dvr_probe(struct device *device, ndev->watchdog_timeo = msecs_to_jiffies(watchdog); #ifdef STMMAC_VLAN_TAG_USED /* Both mac100 and gmac support receive VLAN tag detection */ - ndev->features |= NETIF_F_HW_VLAN_CTAG_RX; + ndev->features |= NETIF_F_HW_VLAN_CTAG_RX | NETIF_F_HW_VLAN_STAG_RX; #endif priv->msg_enable = netif_msg_init(debug, default_msg_level); |