From: Dmitry Bezrukov <dmitry.bezru...@aquantia.com>

Signed-off-by: Dmitry Bezrukov <dmitry.bezru...@aquantia.com>
Signed-off-by: Igor Russkikh <igor.russk...@aquantia.com>
---
 drivers/net/usb/aqc111.c | 17 +++++++++++++++++
 drivers/net/usb/aqc111.h | 12 +++++++++++-
 2 files changed, 28 insertions(+), 1 deletion(-)

diff --git a/drivers/net/usb/aqc111.c b/drivers/net/usb/aqc111.c
index 65f65fd043f2..778d8199031e 100644
--- a/drivers/net/usb/aqc111.c
+++ b/drivers/net/usb/aqc111.c
@@ -534,6 +534,7 @@ static int aqc111_bind(struct usbnet *dev, struct 
usb_interface *intf)
 
        dev->net->hw_features |= AQ_SUPPORT_HW_FEATURE;
        dev->net->features |= AQ_SUPPORT_FEATURE;
+       dev->net->vlan_features |= AQ_SUPPORT_VLAN_FEATURE;
 
        aqc111_read_fw_version(dev, aqc111_data);
        aqc111_data->autoneg = AUTONEG_ENABLE;
@@ -810,6 +811,7 @@ static int aqc111_reset(struct usbnet *dev)
 
        dev->net->hw_features |= AQ_SUPPORT_HW_FEATURE;
        dev->net->features |= AQ_SUPPORT_FEATURE;
+       dev->net->vlan_features |= AQ_SUPPORT_VLAN_FEATURE;
 
        /* Power up ethernet PHY */
        aqc111_data->phy_cfg = AQ_PHY_POWER_EN;
@@ -912,6 +914,7 @@ static int aqc111_rx_fixup(struct usbnet *dev, struct 
sk_buff *skb)
        u32 desc_offset = 0; /*RX Header Offset*/
        u16 pkt_count = 0;
        u64 desc_hdr = 0;
+       u16 vlan_tag = 0;
        u32 skb_len = 0;
 
        if (!skb)
@@ -984,6 +987,12 @@ static int aqc111_rx_fixup(struct usbnet *dev, struct 
sk_buff *skb)
                if (aqc111_data->rx_checksum)
                        aqc111_rx_checksum(new_skb, pkt_desc);
 
+               if (*pkt_desc & AQ_RX_PD_VLAN) {
+                       vlan_tag = *pkt_desc >> AQ_RX_PD_VLAN_SHIFT;
+                       __vlan_hwaccel_put_tag(new_skb, htons(ETH_P_8021Q),
+                                              vlan_tag & VLAN_VID_MASK);
+               }
+
                usbnet_skb_return(dev, new_skb);
                if (pkt_count == 0)
                        break;
@@ -1011,6 +1020,7 @@ static struct sk_buff *aqc111_tx_fixup(struct usbnet 
*dev, struct sk_buff *skb,
        int headroom = 0;
        int tailroom = 0;
        u64 tx_desc = 0;
+       u16 tci = 0;
 
        /*Length of actual data*/
        tx_desc |= skb->len & AQ_TX_DESC_LEN_MASK;
@@ -1028,6 +1038,13 @@ static struct sk_buff *aqc111_tx_fixup(struct usbnet 
*dev, struct sk_buff *skb,
                tx_desc |= AQ_TX_DESC_DROP_PADD;
        }
 
+       /* Vlan Tag */
+       if (vlan_get_tag(skb, &tci) >= 0) {
+               tx_desc |= AQ_TX_DESC_VLAN;
+               tx_desc |= ((u64)tci & AQ_TX_DESC_VLAN_MASK) <<
+                          AQ_TX_DESC_VLAN_SHIFT;
+       }
+
        if (!dev->can_dma_sg && (dev->net->features & NETIF_F_SG) &&
            skb_linearize(skb))
                return NULL;
diff --git a/drivers/net/usb/aqc111.h b/drivers/net/usb/aqc111.h
index fdd7c1059fac..69113fb8d25f 100644
--- a/drivers/net/usb/aqc111.h
+++ b/drivers/net/usb/aqc111.h
@@ -68,12 +68,17 @@
 /* Feature. ********************************************/
 #define AQ_SUPPORT_FEATURE     (NETIF_F_SG | NETIF_F_IP_CSUM |\
                                 NETIF_F_IPV6_CSUM | NETIF_F_RXCSUM |\
-                                NETIF_F_TSO)
+                                NETIF_F_TSO | NETIF_F_HW_VLAN_CTAG_TX |\
+                                NETIF_F_HW_VLAN_CTAG_RX)
 
 #define AQ_SUPPORT_HW_FEATURE  (NETIF_F_SG | NETIF_F_IP_CSUM |\
                                 NETIF_F_IPV6_CSUM | NETIF_F_RXCSUM |\
                                 NETIF_F_TSO)
 
+#define AQ_SUPPORT_VLAN_FEATURE (NETIF_F_SG | NETIF_F_IP_CSUM |\
+                                NETIF_F_IPV6_CSUM | NETIF_F_RXCSUM |\
+                                NETIF_F_TSO)
+
 /* SFR Reg. ********************************************/
 
 #define SFR_GENERAL_STATUS             0x03
@@ -211,8 +216,11 @@ struct aqc111_data {
 /* TX Descriptor */
 #define AQ_TX_DESC_LEN_MASK    0x1FFFFF
 #define AQ_TX_DESC_DROP_PADD   BIT(28)
+#define AQ_TX_DESC_VLAN                BIT(29)
 #define AQ_TX_DESC_MSS_MASK    0x7FFF
 #define AQ_TX_DESC_MSS_SHIFT   0x20
+#define AQ_TX_DESC_VLAN_MASK   0xFFFF
+#define AQ_TX_DESC_VLAN_SHIFT  0x30
 
 #define AQ_RX_HW_PAD                   0x02
 
@@ -226,10 +234,12 @@ struct aqc111_data {
 #define AQ_RX_PD_L3_IP         0x20
 #define AQ_RX_PD_L3_IP6                0x40
 
+#define AQ_RX_PD_VLAN          BIT(10)
 #define AQ_RX_PD_RX_OK         BIT(11)
 #define AQ_RX_PD_DROP          BIT(31)
 #define AQ_RX_PD_LEN_MASK      0x7FFF0000
 #define AQ_RX_PD_LEN_SHIFT     0x10
+#define AQ_RX_PD_VLAN_SHIFT    0x20
 
 /* RX Descriptor header */
 #define AQ_RX_DH_PKT_CNT_MASK          0x1FFF
-- 
2.7.4

Reply via email to