[v2,net-next,15/21] net: usb: aqc111: Add support for VLAN_CTAG_TX/RX offload

Commit Message

Igor Russkikh Nov. 13, 2018, 2:45 p.m. UTC

From: Dmitry Bezrukov <[e-mail]>

Signed-off-by: Dmitry Bezrukov <[e-mail]>
Signed-off-by: Igor Russkikh <[e-mail]>
---
 drivers/net/usb/aqc111.c | 17 +++++++++++++++++
 drivers/net/usb/aqc111.h | 12 +++++++++++-
 2 files changed, 28 insertions(+), 1 deletion(-)

Patch

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