4 files changed, 24 insertions, 6 deletions

diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
index 46ddd432cdf..6db997c78a5 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
@@ -530,6 +530,7 @@ struct adapter {
 	struct net_device *port[MAX_NPORTS];
 	u8 chan_map[NCHAN];                   /* channel -> port map */
 
+	u32 filter_mode;
 	unsigned int l2t_start;
 	unsigned int l2t_end;
 	struct l2t_data *l2t;
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
index a7830a1d8b8..f0718e1a836 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
@@ -2670,7 +2670,8 @@ static int tid_init(struct tid_info *t)
  *	Returns <0 on error and one of the %NET_XMIT_* values on success.
  */
 int cxgb4_create_server(const struct net_device *dev, unsigned int stid,
-			__be32 sip, __be16 sport, unsigned int queue)
+			__be32 sip, __be16 sport, __be16 vlan,
+			unsigned int queue)
 {
 	unsigned int chan;
 	struct sk_buff *skb;
@@ -3043,7 +3044,7 @@ static void uld_attach(struct adapter *adap, unsigned int uld)
 	lli.ucq_density = 1 << QUEUESPERPAGEPF0_GET(
 			t4_read_reg(adap, SGE_INGRESS_QUEUES_PER_PAGE_PF) >>
 			(adap->fn * 4));
-	lli.filt_mode = tp_vlan_pri_map;
+	lli.filt_mode = adap->filter_mode;
 	/* MODQ_REQ_MAP sets queues 0-3 to chan 0-3 */
 	for (i = 0; i < NCHAN; i++)
 		lli.tx_modq[i] = i;
@@ -3307,7 +3308,8 @@ static int delete_filter(struct adapter *adapter, unsigned int fidx)
 }
 
 int cxgb4_create_server_filter(const struct net_device *dev, unsigned int stid,
-		__be32 sip, __be16 sport, unsigned int queue)
+		__be32 sip, __be16 sport, __be16 vlan,
+		unsigned int queue, unsigned char port, unsigned char mask)
 {
 	int ret;
 	struct filter_entry *f;
@@ -3339,11 +3341,16 @@ int cxgb4_create_server_filter(const struct net_device *dev, unsigned int stid,
 	f->fs.val.lport = cpu_to_be16(sport);
 	f->fs.mask.lport  = ~0;
 	val = (u8 *)&sip;
-	if ((val[0] | val[1] | val[2] | val[3]) != 0)
+	if ((val[0] | val[1] | val[2] | val[3]) != 0) {
 		for (i = 0; i < 4; i++) {
 			f->fs.val.lip[i] = val[i];
 			f->fs.mask.lip[i] = ~0;
 		}
+		if (adap->filter_mode & F_PORT) {
+			f->fs.val.iport = port;
+			f->fs.mask.iport = mask;
+		}
+	}
 
 	f->fs.dirsteer = 1;
 	f->fs.iq = queue;
@@ -4450,6 +4457,10 @@ static int adap_init0(struct adapter *adap)
 	for (j = 0; j < NCHAN; j++)
 		adap->params.tp.tx_modq[j] = j;
 
+	t4_read_indirect(adap, TP_PIO_ADDR, TP_PIO_DATA,
+			 &adap->filter_mode, 1,
+			 TP_VLAN_PRI_MAP);
+
 	adap->flags |= FW_OK;
 	return 0;
 
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h
index 065bbd5b241..e2bbc7f3e2d 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_uld.h
@@ -38,6 +38,7 @@
 #include <linux/cache.h>
 #include <linux/spinlock.h>
 #include <linux/skbuff.h>
+#include <linux/inetdevice.h>
 #include <linux/atomic.h>
 
 /* CPL message priority levels */
@@ -151,9 +152,12 @@ void cxgb4_remove_tid(struct tid_info *t, unsigned int qid, unsigned int tid);
 struct in6_addr;
 
 int cxgb4_create_server(const struct net_device *dev, unsigned int stid,
-			__be32 sip, __be16 sport, unsigned int queue);
+			__be32 sip, __be16 sport, __be16 vlan,
+			unsigned int queue);
 int cxgb4_create_server_filter(const struct net_device *dev, unsigned int stid,
-			       __be32 sip, __be16 sport, unsigned int queue);
+			       __be32 sip, __be16 sport, __be16 vlan,
+			       unsigned int queue,
+			       unsigned char port, unsigned char mask);
 int cxgb4_remove_server_filter(const struct net_device *dev, unsigned int stid,
 			       unsigned int queue, bool ipv6);
 static inline void set_wr_txq(struct sk_buff *skb, int prio, int queue)
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h b/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h
index a2c29f7b7aa..83ec5f7844a 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h
@@ -1098,5 +1098,7 @@
 #define A_TP_TX_SCHED_PCMD 0x25
 
 #define S_PORT    1
+#define V_PORT(x) ((x) << S_PORT)
+#define F_PORT    V_PORT(1U)
 
 #endif /* __T4_REGS_H */