net: aquantia: add ethertype and PCP to rx flow filters

L2 EtherType filters allows to filter packet by EtherType field or
both EtherType and User Priority (PCP) field of 802.1Q.
UserPriority (vlan) parameter must be accompanied by mask 0x1FFF. That
is to distinguish VLAN filter from L2 Ethertype filter with
UserPriority since both User Priority and VLAN ID are passed in the
same 'vlan' parameter.

Example:
To add a filter that directs IP4 packess of priority 3 to queue 3:
ethtool -N <ethX> flow-type ether proto 0x800 vlan 0x600 m 0x1FFF \
action 3 loc 16

Signed-off-by: Dmitry Bogdanov <dmitry.bogdanov@aquantia.com>
Signed-off-by: Igor Russkikh <igor.russkikh@aquantia.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

5 files changed, 133 insertions, 4 deletions

diff --git a/drivers/net/ethernet/aquantia/atlantic/aq_filters.c b/drivers/net/ethernet/aquantia/atlantic/aq_filters.c
index c5240bc487b4..b987c6717af6 100644
--- a/drivers/net/ethernet/aquantia/atlantic/aq_filters.c
+++ b/drivers/net/ethernet/aquantia/atlantic/aq_filters.c
@@ -120,6 +120,30 @@ static int aq_check_approve_fl3l4(struct aq_nic_s *aq_nic,
 }
 
 static int __must_check
+aq_check_approve_fl2(struct aq_nic_s *aq_nic,
+		     struct aq_hw_rx_fltrs_s *rx_fltrs,
+		     struct ethtool_rx_flow_spec *fsp)
+{
+	if (fsp->location < AQ_RX_FIRST_LOC_FETHERT ||
+	    fsp->location > AQ_RX_LAST_LOC_FETHERT) {
+		netdev_err(aq_nic->ndev,
+			   "ethtool: location must be in range [%d, %d]",
+			   AQ_RX_FIRST_LOC_FETHERT,
+			   AQ_RX_LAST_LOC_FETHERT);
+		return -EINVAL;
+	}
+
+	if (be16_to_cpu(fsp->m_ext.vlan_tci) == VLAN_PRIO_MASK &&
+	    fsp->m_u.ether_spec.h_proto == 0U) {
+		netdev_err(aq_nic->ndev,
+			   "ethtool: proto (ether_type) parameter must be specfied");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int __must_check
 aq_check_approve_fvlan(struct aq_nic_s *aq_nic,
 		       struct aq_hw_rx_fltrs_s *rx_fltrs,
 		       struct ethtool_rx_flow_spec *fsp)
@@ -152,6 +176,8 @@ aq_check_filter(struct aq_nic_s *aq_nic,
 	if (fsp->flow_type & FLOW_EXT) {
 		if (be16_to_cpu(fsp->m_ext.vlan_tci) == VLAN_VID_MASK) {
 			err = aq_check_approve_fvlan(aq_nic, rx_fltrs, fsp);
+		} else if (be16_to_cpu(fsp->m_ext.vlan_tci) == VLAN_PRIO_MASK) {
+			err = aq_check_approve_fl2(aq_nic, rx_fltrs, fsp);
 		} else {
 			netdev_err(aq_nic->ndev,
 				   "ethtool: invalid vlan mask 0x%x specified",
@@ -161,7 +187,7 @@ aq_check_filter(struct aq_nic_s *aq_nic,
 	} else {
 		switch (fsp->flow_type & ~FLOW_EXT) {
 		case ETHER_FLOW:
-			err = -EOPNOTSUPP;
+			err = aq_check_approve_fl2(aq_nic, rx_fltrs, fsp);
 			break;
 		case TCP_V4_FLOW:
 		case UDP_V4_FLOW:
@@ -210,6 +236,10 @@ aq_rule_is_not_support(struct aq_nic_s *aq_nic,
 		netdev_err(aq_nic->ndev,
 			   "ethtool: The specified tos tclass are not supported\n");
 		rule_is_not_support = true;
+	} else if (fsp->flow_type & FLOW_MAC_EXT) {
+		netdev_err(aq_nic->ndev,
+			   "ethtool: MAC_EXT is not supported");
+		rule_is_not_support = true;
 	}
 
 	return rule_is_not_support;
@@ -259,6 +289,48 @@ aq_check_rule(struct aq_nic_s *aq_nic,
 	return err;
 }
 
+static void aq_set_data_fl2(struct aq_nic_s *aq_nic,
+			    struct aq_rx_filter *aq_rx_fltr,
+			    struct aq_rx_filter_l2 *data, bool add)
+{
+	const struct ethtool_rx_flow_spec *fsp = &aq_rx_fltr->aq_fsp;
+
+	memset(data, 0, sizeof(*data));
+
+	data->location = fsp->location - AQ_RX_FIRST_LOC_FETHERT;
+
+	if (fsp->ring_cookie != RX_CLS_FLOW_DISC)
+		data->queue = fsp->ring_cookie;
+	else
+		data->queue = -1;
+
+	data->ethertype = be16_to_cpu(fsp->h_u.ether_spec.h_proto);
+	data->user_priority_en = be16_to_cpu(fsp->m_ext.vlan_tci)
+				 == VLAN_PRIO_MASK;
+	data->user_priority = (be16_to_cpu(fsp->h_ext.vlan_tci)
+			       & VLAN_PRIO_MASK) >> VLAN_PRIO_SHIFT;
+}
+
+static int aq_add_del_fether(struct aq_nic_s *aq_nic,
+			     struct aq_rx_filter *aq_rx_fltr, bool add)
+{
+	struct aq_rx_filter_l2 data;
+	struct aq_hw_s *aq_hw = aq_nic->aq_hw;
+	const struct aq_hw_ops *aq_hw_ops = aq_nic->aq_hw_ops;
+
+	aq_set_data_fl2(aq_nic, aq_rx_fltr, &data, add);
+
+	if (unlikely(!aq_hw_ops->hw_filter_l2_set))
+		return -EOPNOTSUPP;
+	if (unlikely(!aq_hw_ops->hw_filter_l2_clear))
+		return -EOPNOTSUPP;
+
+	if (add)
+		return aq_hw_ops->hw_filter_l2_set(aq_hw, &data);
+	else
+		return aq_hw_ops->hw_filter_l2_clear(aq_hw, &data);
+}
+
 static int aq_set_data_fvlan(struct aq_nic_s *aq_nic,
 			     struct aq_rx_filter *aq_rx_fltr,
 			     struct aq_rx_filter_vlan *aq_vlans, bool add)
@@ -424,13 +496,16 @@ static int aq_add_del_rule(struct aq_nic_s *aq_nic,
 		    == VLAN_VID_MASK) {
 			aq_rx_fltr->type = aq_rx_filter_vlan;
 			err = aq_add_del_fvlan(aq_nic, aq_rx_fltr, add);
-		} else {
-			err = -EINVAL;
+		} else if (be16_to_cpu(aq_rx_fltr->aq_fsp.m_ext.vlan_tci)
+			== VLAN_PRIO_MASK) {
+			aq_rx_fltr->type = aq_rx_filter_ethertype;
+			err = aq_add_del_fether(aq_nic, aq_rx_fltr, add);
 		}
 	} else {
 		switch (aq_rx_fltr->aq_fsp.flow_type & ~FLOW_EXT) {
 		case ETHER_FLOW:
-			err = -EOPNOTSUPP;
+			aq_rx_fltr->type = aq_rx_filter_ethertype;
+			err = aq_add_del_fether(aq_nic, aq_rx_fltr, add);
 			break;
 		case TCP_V4_FLOW:
 		case UDP_V4_FLOW:
diff --git a/drivers/net/ethernet/aquantia/atlantic/aq_filters.h b/drivers/net/ethernet/aquantia/atlantic/aq_filters.h
index bbaf331a55bb..4c57c26fd3f0 100644
--- a/drivers/net/ethernet/aquantia/atlantic/aq_filters.h
+++ b/drivers/net/ethernet/aquantia/atlantic/aq_filters.h
@@ -9,6 +9,7 @@
 #include "aq_nic.h"
 
 enum aq_rx_filter_type {
+	aq_rx_filter_ethertype,
 	aq_rx_filter_vlan,
 	aq_rx_filter_l3l4
 };
diff --git a/drivers/net/ethernet/aquantia/atlantic/aq_hw.h b/drivers/net/ethernet/aquantia/atlantic/aq_hw.h
index d31474c8a498..b0a48956e22f 100644
--- a/drivers/net/ethernet/aquantia/atlantic/aq_hw.h
+++ b/drivers/net/ethernet/aquantia/atlantic/aq_hw.h
@@ -20,6 +20,8 @@
 
 #define AQ_RX_FIRST_LOC_FVLANID     0U
 #define AQ_RX_LAST_LOC_FVLANID	   15U
+#define AQ_RX_FIRST_LOC_FETHERT    16U
+#define AQ_RX_LAST_LOC_FETHERT	   31U
 #define AQ_RX_FIRST_LOC_FL3L4	   32U
 #define AQ_RX_LAST_LOC_FL3L4	   39U
 #define AQ_RX_MAX_RXNFC_LOC	   AQ_RX_LAST_LOC_FL3L4
@@ -198,6 +200,12 @@ struct aq_hw_ops {
 	int (*hw_filter_l3l4_clear)(struct aq_hw_s *self,
 				    struct aq_rx_filter_l3l4 *data);
 
+	int (*hw_filter_l2_set)(struct aq_hw_s *self,
+				struct aq_rx_filter_l2 *data);
+
+	int (*hw_filter_l2_clear)(struct aq_hw_s *self,
+				  struct aq_rx_filter_l2 *data);
+
 	int (*hw_filter_vlan_set)(struct aq_hw_s *self,
 				  struct aq_rx_filter_vlan *aq_vlans);
 
diff --git a/drivers/net/ethernet/aquantia/atlantic/hw_atl/hw_atl_b0.c b/drivers/net/ethernet/aquantia/atlantic/hw_atl/hw_atl_b0.c
index 4ee30fa2e36b..a8777751d09b 100644
--- a/drivers/net/ethernet/aquantia/atlantic/hw_atl/hw_atl_b0.c
+++ b/drivers/net/ethernet/aquantia/atlantic/hw_atl/hw_atl_b0.c
@@ -1003,6 +1003,41 @@ static int hw_atl_b0_hw_fl3l4_set(struct aq_hw_s *self,
 	return aq_hw_err_from_flags(self);
 }
 
+static int hw_atl_b0_hw_fl2_set(struct aq_hw_s *self,
+				struct aq_rx_filter_l2 *data)
+{
+	hw_atl_rpf_etht_flr_en_set(self, 1U, data->location);
+	hw_atl_rpf_etht_flr_set(self, data->ethertype, data->location);
+	hw_atl_rpf_etht_user_priority_en_set(self,
+					     !!data->user_priority_en,
+					     data->location);
+	if (data->user_priority_en)
+		hw_atl_rpf_etht_user_priority_set(self,
+						  data->user_priority,
+						  data->location);
+
+	if (data->queue < 0) {
+		hw_atl_rpf_etht_flr_act_set(self, 0U, data->location);
+		hw_atl_rpf_etht_rx_queue_en_set(self, 0U, data->location);
+	} else {
+		hw_atl_rpf_etht_flr_act_set(self, 1U, data->location);
+		hw_atl_rpf_etht_rx_queue_en_set(self, 1U, data->location);
+		hw_atl_rpf_etht_rx_queue_set(self, data->queue, data->location);
+	}
+
+	return aq_hw_err_from_flags(self);
+}
+
+static int hw_atl_b0_hw_fl2_clear(struct aq_hw_s *self,
+				  struct aq_rx_filter_l2 *data)
+{
+	hw_atl_rpf_etht_flr_en_set(self, 0U, data->location);
+	hw_atl_rpf_etht_flr_set(self, 0U, data->location);
+	hw_atl_rpf_etht_user_priority_en_set(self, 0U, data->location);
+
+	return aq_hw_err_from_flags(self);
+}
+
 /**
  * @brief Set VLAN filter table
  * @details Configure VLAN filter table to accept (and assign the queue) traffic
@@ -1063,6 +1098,8 @@ const struct aq_hw_ops hw_atl_ops_b0 = {
 	.hw_ring_rx_init             = hw_atl_b0_hw_ring_rx_init,
 	.hw_ring_tx_init             = hw_atl_b0_hw_ring_tx_init,
 	.hw_packet_filter_set        = hw_atl_b0_hw_packet_filter_set,
+	.hw_filter_l2_set            = hw_atl_b0_hw_fl2_set,
+	.hw_filter_l2_clear          = hw_atl_b0_hw_fl2_clear,
 	.hw_filter_l3l4_set          = hw_atl_b0_hw_fl3l4_set,
 	.hw_filter_vlan_set          = hw_atl_b0_hw_vlan_set,
 	.hw_multicast_list_set       = hw_atl_b0_hw_multicast_list_set,
diff --git a/drivers/net/ethernet/aquantia/atlantic/hw_atl/hw_atl_utils.h b/drivers/net/ethernet/aquantia/atlantic/hw_atl/hw_atl_utils.h
index 3c5b81420361..48278e333462 100644
--- a/drivers/net/ethernet/aquantia/atlantic/hw_atl/hw_atl_utils.h
+++ b/drivers/net/ethernet/aquantia/atlantic/hw_atl/hw_atl_utils.h
@@ -252,6 +252,14 @@ struct aq_rx_filter_vlan {
 	u8 queue;
 };
 
+struct aq_rx_filter_l2 {
+	s8 queue;
+	u8 location;
+	u8 user_priority_en;
+	u8 user_priority;
+	u16 ethertype;
+};
+
 struct aq_rx_filter_l3l4 {
 	u32 cmd;
 	u8 location;