[PATCH net-next 16/19] net: usb: aqc111: Add RX VLAN filtering support

[Igor Russkikh <Igor.Russkikh@aquantia.com>]

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

Signed-off-by: Dmitry Bezrukov <dmitry.bezrukov@aquantia.com>
Signed-off-by: Igor Russkikh <igor.russkikh@aquantia.com>
---
 drivers/net/usb/aqc111.c | 90 ++++++++++++++++++++++++++++++++++++++++++++++++
 drivers/net/usb/aqc111.h |  2 +-
 2 files changed, 91 insertions(+), 1 deletion(-)

diff --git a/drivers/net/usb/aqc111.c b/drivers/net/usb/aqc111.c
index a9051dd7c5bd..9908b0a04da6 100644
--- a/drivers/net/usb/aqc111.c
+++ b/drivers/net/usb/aqc111.c
@@ -351,6 +351,57 @@ static int aqc111_set_mac_addr(struct net_device *net, void *p)
 				ETH_ALEN, net->dev_addr);
 }
 
+static int aqc111_vlan_rx_kill_vid(struct net_device *net,
+				   __be16 proto, u16 vid)
+{
+	struct usbnet *dev = netdev_priv(net);
+	u8 reg8 = 0;
+	u16 reg16 = 0;
+	u8 vlan_ctrl = 0;
+
+	aqc111_read_cmd(dev, AQ_ACCESS_MAC, SFR_VLAN_ID_CONTROL, 1, 1, &reg8);
+	vlan_ctrl = reg8;
+
+	/* Address */
+	reg8 = (vid / 16);
+	aqc111_write_cmd(dev, AQ_ACCESS_MAC, SFR_VLAN_ID_ADDRESS, 1, 1, &reg8);
+	/* Data */
+	reg8 = vlan_ctrl | SFR_VLAN_CONTROL_RD;
+	aqc111_write_cmd(dev, AQ_ACCESS_MAC, SFR_VLAN_ID_CONTROL, 1, 1, &reg8);
+	aqc111_read_cmd(dev, AQ_ACCESS_MAC, SFR_VLAN_ID_DATA0, 2, 2, &reg16);
+	reg16 &= ~(1 << (vid % 16));
+	aqc111_write_cmd(dev, AQ_ACCESS_MAC, SFR_VLAN_ID_DATA0, 2, 2, &reg16);
+	reg8 = vlan_ctrl | SFR_VLAN_CONTROL_WE;
+	aqc111_write_cmd(dev, AQ_ACCESS_MAC, SFR_VLAN_ID_CONTROL, 1, 1, &reg8);
+
+	return 0;
+}
+
+static int aqc111_vlan_rx_add_vid(struct net_device *net, __be16 proto, u16 vid)
+{
+	struct usbnet *dev = netdev_priv(net);
+	u8 reg8 = 0;
+	u16 reg16 = 0;
+	u8 vlan_ctrl = 0;
+
+	aqc111_read_cmd(dev, AQ_ACCESS_MAC, SFR_VLAN_ID_CONTROL, 1, 1, &reg8);
+	vlan_ctrl = reg8;
+
+	/* Address */
+	reg8 = (vid / 16);
+	aqc111_write_cmd(dev, AQ_ACCESS_MAC, SFR_VLAN_ID_ADDRESS, 1, 1, &reg8);
+	/* Data */
+	reg8 = vlan_ctrl | SFR_VLAN_CONTROL_RD;
+	aqc111_write_cmd(dev, AQ_ACCESS_MAC, SFR_VLAN_ID_CONTROL, 1, 1, &reg8);
+	aqc111_read_cmd(dev, AQ_ACCESS_MAC, SFR_VLAN_ID_DATA0, 2, 2, &reg16);
+	reg16 |= (1 << (vid % 16));
+	aqc111_write_cmd(dev, AQ_ACCESS_MAC, SFR_VLAN_ID_DATA0, 2, 2, &reg16);
+	reg8 = vlan_ctrl | SFR_VLAN_CONTROL_WE;
+	aqc111_write_cmd(dev, AQ_ACCESS_MAC, SFR_VLAN_ID_CONTROL, 1, 1, &reg8);
+
+	return 0;
+}
+
 static void aqc111_set_rx_mode(struct net_device *net)
 {
 	struct usbnet *dev = netdev_priv(net);
@@ -393,6 +444,7 @@ static int aqc111_set_features(struct net_device *net,
 			       netdev_features_t features)
 {
 	u8 reg8 = 0;
+	u16 reg16 = 0;
 	struct usbnet *dev = netdev_priv(net);
 	struct aqc111_data *aqc111_data = (struct aqc111_data *)dev->data[0];
 	netdev_features_t changed = net->features ^ features;
@@ -426,6 +478,39 @@ static int aqc111_set_features(struct net_device *net,
 		aqc111_write_cmd(dev, AQ_ACCESS_MAC, SFR_RXCOE_CTL,
 				 1, 1, &reg8);
 	}
+	if (changed & NETIF_F_HW_VLAN_CTAG_FILTER) {
+		if (features & NETIF_F_HW_VLAN_CTAG_FILTER) {
+			u16 i = 0;
+
+			for (i = 0; i < 256; i++) {
+				/* Address */
+				reg8 = i;
+				aqc111_write_cmd(dev, AQ_ACCESS_MAC,
+						 SFR_VLAN_ID_ADDRESS,
+						 1, 1, &reg8);
+				/* Data */
+				aqc111_write_cmd(dev, AQ_ACCESS_MAC,
+						 SFR_VLAN_ID_DATA0,
+						 2, 2, &reg16);
+				reg8 = SFR_VLAN_CONTROL_WE;
+				aqc111_write_cmd(dev, AQ_ACCESS_MAC,
+						 SFR_VLAN_ID_CONTROL,
+						 1, 1, &reg8);
+			}
+			aqc111_read_cmd(dev, AQ_ACCESS_MAC, SFR_VLAN_ID_CONTROL,
+					1, 1, &reg8);
+			reg8 |= SFR_VLAN_CONTROL_VFE;
+			aqc111_write_cmd(dev, AQ_ACCESS_MAC,
+					 SFR_VLAN_ID_CONTROL, 1, 1, &reg8);
+		} else {
+			aqc111_read_cmd(dev, AQ_ACCESS_MAC, SFR_VLAN_ID_CONTROL,
+					1, 1, &reg8);
+			reg8 &= ~SFR_VLAN_CONTROL_VFE;
+			aqc111_write_cmd(dev, AQ_ACCESS_MAC,
+					 SFR_VLAN_ID_CONTROL, 1, 1, &reg8);
+		}
+	}
+
 	return 0;
 }
 
@@ -438,6 +523,8 @@ static const struct net_device_ops aqc111_netdev_ops = {
 	.ndo_change_mtu		= aqc111_change_mtu,
 	.ndo_set_mac_address	= aqc111_set_mac_addr,
 	.ndo_validate_addr	= eth_validate_addr,
+	.ndo_vlan_rx_add_vid	= aqc111_vlan_rx_add_vid,
+	.ndo_vlan_rx_kill_vid	= aqc111_vlan_rx_kill_vid,
 	.ndo_set_rx_mode	= aqc111_set_rx_mode,
 	.ndo_set_features	= aqc111_set_features,
 };
@@ -726,6 +813,8 @@ static int aqc111_link_reset(struct usbnet *dev)
 
 		/* Vlan Tag Filter */
 		reg8 = SFR_VLAN_CONTROL_VSO;
+		if (dev->net->features & NETIF_F_HW_VLAN_CTAG_FILTER)
+			reg8 |= SFR_VLAN_CONTROL_VFE;
 
 		aqc111_write_cmd(dev, AQ_ACCESS_MAC, SFR_VLAN_ID_CONTROL,
 				 1, 1, &reg8);
@@ -994,6 +1083,7 @@ static int aqc111_rx_fixup(struct usbnet *dev, struct sk_buff *skb)
 		new_skb->truesize = new_skb->len + sizeof(struct sk_buff);
 		if (aqc111_data->rx_checksum)
 			aqc111_rx_checksum(new_skb, &pkt_desc);
+
 		if (pkt_desc->vlan_ind)
 			__vlan_hwaccel_put_tag(new_skb,
 					       htons(ETH_P_8021Q),
diff --git a/drivers/net/usb/aqc111.h b/drivers/net/usb/aqc111.h
index e33459d90ddd..a7ccee225e93 100644
--- a/drivers/net/usb/aqc111.h
+++ b/drivers/net/usb/aqc111.h
@@ -73,7 +73,7 @@
 
 #define AQ_SUPPORT_HW_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_FILTER)
 
 #define AQ_SUPPORT_VLAN_FEATURE (NETIF_F_SG | NETIF_F_IP_CSUM |\
 				 NETIF_F_IPV6_CSUM | NETIF_F_RXCSUM |\
-- 
2.7.4