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 46ddd432cdf2..6db997c78a5f 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 a7830a1d8b89..f0718e1a8369 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 065bbd5b2411..e2bbc7f3e2de 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 a2c29f7b7aa1..83ec5f7844ac 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 */