1 files changed, 43 insertions, 37 deletions

diff --git a/drivers/net/wireless/brcm80211/brcmfmac/fwsignal.c b/drivers/net/wireless/brcm80211/brcmfmac/fwsignal.c
index c3e7d76dbf35..699908de314a 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/fwsignal.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/fwsignal.c
@@ -476,6 +476,7 @@ struct brcmf_fws_info {
         bool bus_flow_blocked;
         bool creditmap_received;
         u8 mode;
+        bool avoid_queueing;
 };
 
 /*
@@ -1369,13 +1370,12 @@ done:
 }
 
 static int brcmf_fws_txstatus_suppressed(struct brcmf_fws_info *fws, int fifo,
-                                         struct sk_buff *skb, u32 genbit,
-                                         u16 seq)
+                                         struct sk_buff *skb, u8 ifidx,
+                                         u32 genbit, u16 seq)
 {
         struct brcmf_fws_mac_descriptor *entry = brcmf_skbcb(skb)->mac;
         u32 hslot;
         int ret;
-        u8 ifidx;
 
         hslot = brcmf_skb_htod_tag_get_field(skb, HSLOT);
 
@@ -1389,29 +1389,21 @@ static int brcmf_fws_txstatus_suppressed(struct brcmf_fws_info *fws, int fifo,
 
         entry->generation = genbit;
 
-        ret = brcmf_proto_hdrpull(fws->drvr, false, &ifidx, skb);
-        if (ret == 0) {
-                brcmf_skb_htod_tag_set_field(skb, GENERATION, genbit);
-                brcmf_skbcb(skb)->htod_seq = seq;
-                if (brcmf_skb_htod_seq_get_field(skb, FROMFW)) {
-                        brcmf_skb_htod_seq_set_field(skb, FROMDRV, 1);
-                        brcmf_skb_htod_seq_set_field(skb, FROMFW, 0);
-                } else {
-                        brcmf_skb_htod_seq_set_field(skb, FROMDRV, 0);
-                }
-                ret = brcmf_fws_enq(fws, BRCMF_FWS_SKBSTATE_SUPPRESSED, fifo,
-                                    skb);
+        brcmf_skb_htod_tag_set_field(skb, GENERATION, genbit);
+        brcmf_skbcb(skb)->htod_seq = seq;
+        if (brcmf_skb_htod_seq_get_field(skb, FROMFW)) {
+                brcmf_skb_htod_seq_set_field(skb, FROMDRV, 1);
+                brcmf_skb_htod_seq_set_field(skb, FROMFW, 0);
+        } else {
+                brcmf_skb_htod_seq_set_field(skb, FROMDRV, 0);
         }
+        ret = brcmf_fws_enq(fws, BRCMF_FWS_SKBSTATE_SUPPRESSED, fifo, skb);
 
         if (ret != 0) {
-                /* suppress q is full or hdrpull failed, drop this packet */
-                brcmf_fws_hanger_poppkt(&fws->hanger, hslot, &skb,
-                                        true);
+                /* suppress q is full drop this packet */
+                brcmf_fws_hanger_poppkt(&fws->hanger, hslot, &skb, true);
         } else {
-                /*
-                 * Mark suppressed to avoid a double free during
-                 * wlfc cleanup
-                 */
+                /* Mark suppressed to avoid a double free during wlfc cleanup */
                 brcmf_fws_hanger_mark_suppressed(&fws->hanger, hslot);
         }
 
@@ -1428,6 +1420,7 @@ brcmf_fws_txs_process(struct brcmf_fws_info *fws, u8 flags, u32 hslot,
         struct sk_buff *skb;
         struct brcmf_skbuff_cb *skcb;
         struct brcmf_fws_mac_descriptor *entry = NULL;
+        u8 ifidx;
 
         brcmf_dbg(DATA, "flags %d\n", flags);
 
@@ -1476,12 +1469,15 @@ brcmf_fws_txs_process(struct brcmf_fws_info *fws, u8 flags, u32 hslot,
         }
         brcmf_fws_macdesc_return_req_credit(skb);
 
+        if (brcmf_proto_hdrpull(fws->drvr, false, &ifidx, skb)) {
+                brcmu_pkt_buf_free_skb(skb);
+                return -EINVAL;
+        }
         if (!remove_from_hanger)
-                ret = brcmf_fws_txstatus_suppressed(fws, fifo, skb, genbit,
-                                                    seq);
-
+                ret = brcmf_fws_txstatus_suppressed(fws, fifo, skb, ifidx,
+                                                    genbit, seq);
         if (remove_from_hanger || ret)
-                brcmf_txfinalize(fws->drvr, skb, true);
+                brcmf_txfinalize(fws->drvr, skb, ifidx, true);
 
         return 0;
 }
@@ -1868,7 +1864,7 @@ int brcmf_fws_process_skb(struct brcmf_if *ifp, struct sk_buff *skb)
         struct ethhdr *eh = (struct ethhdr *)(skb->data);
         int fifo = BRCMF_FWS_FIFO_BCMC;
         bool multicast = is_multicast_ether_addr(eh->h_dest);
-        bool pae = eh->h_proto == htons(ETH_P_PAE);
+        int rc = 0;
 
         brcmf_dbg(DATA, "tx proto=0x%X\n", ntohs(eh->h_proto));
         /* determine the priority */
@@ -1876,8 +1872,13 @@ int brcmf_fws_process_skb(struct brcmf_if *ifp, struct sk_buff *skb)
                 skb->priority = cfg80211_classify8021d(skb, NULL);
 
         drvr->tx_multicast += !!multicast;
-        if (pae)
-                atomic_inc(&ifp->pend_8021x_cnt);
+
+        if (fws->avoid_queueing) {
+                rc = brcmf_proto_txdata(drvr, ifp->ifidx, 0, skb);
+                if (rc < 0)
+                        brcmf_txfinalize(drvr, skb, ifp->ifidx, false);
+                return rc;
+        }
 
         /* set control buffer information */
         skcb->if_flags = 0;
@@ -1899,15 +1900,12 @@ int brcmf_fws_process_skb(struct brcmf_if *ifp, struct sk_buff *skb)
                 brcmf_fws_schedule_deq(fws);
         } else {
                 brcmf_err("drop skb: no hanger slot\n");
-                if (pae) {
-                        atomic_dec(&ifp->pend_8021x_cnt);
-                        if (waitqueue_active(&ifp->pend_8021x_wait))
-                                wake_up(&ifp->pend_8021x_wait);
-                }
-                brcmu_pkt_buf_free_skb(skb);
+                brcmf_txfinalize(drvr, skb, ifp->ifidx, false);
+                rc = -ENOMEM;
         }
         brcmf_fws_unlock(fws);
-        return 0;
+
+        return rc;
 }
 
 void brcmf_fws_reset_interface(struct brcmf_if *ifp)
@@ -1982,7 +1980,8 @@ static void brcmf_fws_dequeue_worker(struct work_struct *worker)
                                 ret = brcmf_proto_txdata(drvr, ifidx, 0, skb);
                                 brcmf_fws_lock(fws);
                                 if (ret < 0)
-                                        brcmf_txfinalize(drvr, skb, false);
+                                        brcmf_txfinalize(drvr, skb, ifidx,
+                                                         false);
                                 if (fws->bus_flow_blocked)
                                         break;
                         }
@@ -2039,6 +2038,13 @@ int brcmf_fws_init(struct brcmf_pub *drvr)
         fws->drvr = drvr;
         fws->fcmode = fcmode;
 
+        if ((drvr->bus_if->always_use_fws_queue == false) &&
+            (fcmode == BRCMF_FWS_FCMODE_NONE)) {
+                fws->avoid_queueing = true;
+                brcmf_dbg(INFO, "FWS queueing will be avoided\n");
+                return 0;
+        }
+
         fws->fws_wq = create_singlethread_workqueue("brcmf_fws_wq");
         if (fws->fws_wq == NULL) {
                 brcmf_err("workqueue creation failed\n");