Merge tag 'v5.9-rc2' into drm-misc-fixes

Backmerge requested by Tomi for a fix to omap inconsistent
locking state issue, and because we need at least v5.9-rc2 now.

Signed-off-by: Maarten Lankhorst <maarten.lankhorst@linux.intel.com>

17 files changed, 289 insertions, 355 deletions

diff --git a/drivers/staging/wfx/bh.c b/drivers/staging/wfx/bh.c
index 1cbaf8bb4fa3..53ae0b5abcdd 100644
--- a/drivers/staging/wfx/bh.c
+++ b/drivers/staging/wfx/bh.c
@@ -57,7 +57,6 @@ static int rx_helper(struct wfx_dev *wdev, size_t read_len, int *is_cnf)
 	int release_count;
 	int piggyback = 0;
 
-	WARN(read_len < 4, "corrupted read");
 	WARN(read_len > round_down(0xFFF, 2) * sizeof(u16),
 	     "%s: request exceed WFx capability", __func__);
 
@@ -76,20 +75,17 @@ static int rx_helper(struct wfx_dev *wdev, size_t read_len, int *is_cnf)
 	hif = (struct hif_msg *)skb->data;
 	WARN(hif->encrypted & 0x1, "unsupported encryption type");
 	if (hif->encrypted == 0x2) {
-		if (wfx_sl_decode(wdev, (void *)hif)) {
-			dev_kfree_skb(skb);
-			// If frame was a confirmation, expect trouble in next
-			// exchange. However, it is harmless to fail to decode
-			// an indication frame, so try to continue. Anyway,
-			// piggyback is probably correct.
-			return piggyback;
-		}
-		computed_len =
-			round_up(le16_to_cpu(hif->len) - sizeof(hif->len), 16) +
-			sizeof(struct hif_sl_msg) +
-			sizeof(struct hif_sl_tag);
+		if (WARN(read_len < sizeof(struct hif_sl_msg), "corrupted read"))
+			goto err;
+		computed_len = le16_to_cpu(((struct hif_sl_msg *)hif)->len);
+		computed_len = round_up(computed_len - sizeof(u16), 16);
+		computed_len += sizeof(struct hif_sl_msg);
+		computed_len += sizeof(struct hif_sl_tag);
 	} else {
-		computed_len = round_up(le16_to_cpu(hif->len), 2);
+		if (WARN(read_len < sizeof(struct hif_msg), "corrupted read"))
+			goto err;
+		computed_len = le16_to_cpu(hif->len);
+		computed_len = round_up(computed_len, 2);
 	}
 	if (computed_len != read_len) {
 		dev_err(wdev->dev, "inconsistent message length: %zu != %zu\n",
@@ -98,6 +94,16 @@ static int rx_helper(struct wfx_dev *wdev, size_t read_len, int *is_cnf)
 			       hif, read_len, true);
 		goto err;
 	}
+	if (hif->encrypted == 0x2) {
+		if (wfx_sl_decode(wdev, (struct hif_sl_msg *)hif)) {
+			dev_kfree_skb(skb);
+			// If frame was a confirmation, expect trouble in next
+			// exchange. However, it is harmless to fail to decode
+			// an indication frame, so try to continue. Anyway,
+			// piggyback is probably correct.
+			return piggyback;
+		}
+	}
 
 	if (!(hif->id & HIF_ID_IS_INDICATION)) {
 		(*is_cnf)++;
diff --git a/drivers/staging/wfx/bus_spi.c b/drivers/staging/wfx/bus_spi.c
index e8da61fb096b..d19c0478e8be 100644
--- a/drivers/staging/wfx/bus_spi.c
+++ b/drivers/staging/wfx/bus_spi.c
@@ -8,7 +8,6 @@
  */
 #include <linux/module.h>
 #include <linux/delay.h>
-#include <linux/gpio.h>
 #include <linux/gpio/consumer.h>
 #include <linux/spi/spi.h>
 #include <linux/interrupt.h>
@@ -21,10 +20,6 @@
 #include "main.h"
 #include "bh.h"
 
-static int gpio_reset = -2;
-module_param(gpio_reset, int, 0644);
-MODULE_PARM_DESC(gpio_reset, "gpio number for reset. -1 for none.");
-
 #define SET_WRITE 0x7FFF        /* usage: and operation */
 #define SET_READ 0x8000         /* usage: or operation */
 
@@ -211,10 +206,15 @@ static int wfx_spi_probe(struct spi_device *func)
 		bus->need_swab = true;
 	spi_set_drvdata(func, bus);
 
-	bus->gpio_reset = wfx_get_gpio(&func->dev, gpio_reset, "reset");
+	bus->gpio_reset = devm_gpiod_get_optional(&func->dev, "reset",
+						  GPIOD_OUT_LOW);
+	if (IS_ERR(bus->gpio_reset))
+		return PTR_ERR(bus->gpio_reset);
 	if (!bus->gpio_reset) {
-		dev_warn(&func->dev, "try to load firmware anyway\n");
+		dev_warn(&func->dev,
+			 "gpio reset is not defined, trying to load firmware anyway\n");
 	} else {
+		gpiod_set_consumer_name(bus->gpio_reset, "wfx reset");
 		if (spi_get_device_id(func)->driver_data & WFX_RESET_INVERTED)
 			gpiod_toggle_active_low(bus->gpio_reset);
 		gpiod_set_value_cansleep(bus->gpio_reset, 1);
diff --git a/drivers/staging/wfx/data_rx.c b/drivers/staging/wfx/data_rx.c
index 0e959ebc38b5..6fb078880742 100644
--- a/drivers/staging/wfx/data_rx.c
+++ b/drivers/staging/wfx/data_rx.c
@@ -13,55 +13,22 @@
 #include "bh.h"
 #include "sta.h"
 
-static int wfx_drop_encrypt_data(struct wfx_dev *wdev,
-				 const struct hif_ind_rx *arg,
-				 struct sk_buff *skb)
+static void wfx_rx_handle_ba(struct wfx_vif *wvif, struct ieee80211_mgmt *mgmt)
 {
-	struct ieee80211_hdr *frame = (struct ieee80211_hdr *)skb->data;
-	size_t hdrlen = ieee80211_hdrlen(frame->frame_control);
-	size_t iv_len, icv_len;
+	int params, tid;
 
-	/* Oops... There is no fast way to ask mac80211 about
-	 * IV/ICV lengths. Even defineas are not exposed.
-	 */
-	switch (arg->rx_flags.encryp) {
-	case HIF_RI_FLAGS_WEP_ENCRYPTED:
-		iv_len = 4 /* WEP_IV_LEN */;
-		icv_len = 4 /* WEP_ICV_LEN */;
-		break;
-	case HIF_RI_FLAGS_TKIP_ENCRYPTED:
-		iv_len = 8 /* TKIP_IV_LEN */;
-		icv_len = 4 /* TKIP_ICV_LEN */
-			+ 8 /*MICHAEL_MIC_LEN*/;
+	switch (mgmt->u.action.u.addba_req.action_code) {
+	case WLAN_ACTION_ADDBA_REQ:
+		params = le16_to_cpu(mgmt->u.action.u.addba_req.capab);
+		tid = (params & IEEE80211_ADDBA_PARAM_TID_MASK) >> 2;
+		ieee80211_start_rx_ba_session_offl(wvif->vif, mgmt->sa, tid);
 		break;
-	case HIF_RI_FLAGS_AES_ENCRYPTED:
-		iv_len = 8 /* CCMP_HDR_LEN */;
-		icv_len = 8 /* CCMP_MIC_LEN */;
+	case WLAN_ACTION_DELBA:
+		params = le16_to_cpu(mgmt->u.action.u.delba.params);
+		tid = (params &  IEEE80211_DELBA_PARAM_TID_MASK) >> 12;
+		ieee80211_stop_rx_ba_session_offl(wvif->vif, mgmt->sa, tid);
 		break;
-	case HIF_RI_FLAGS_WAPI_ENCRYPTED:
-		iv_len = 18 /* WAPI_HDR_LEN */;
-		icv_len = 16 /* WAPI_MIC_LEN */;
-		break;
-	default:
-		dev_err(wdev->dev, "unknown encryption type %d\n",
-			arg->rx_flags.encryp);
-		return -EIO;
-	}
-
-	/* Firmware strips ICV in case of MIC failure. */
-	if (arg->status == HIF_STATUS_RX_FAIL_MIC)
-		icv_len = 0;
-
-	if (skb->len < hdrlen + iv_len + icv_len) {
-		dev_warn(wdev->dev, "malformed SDU received\n");
-		return -EIO;
 	}
-
-	/* Remove IV, ICV and MIC */
-	skb_trim(skb, skb->len - icv_len);
-	memmove(skb->data + iv_len, skb->data, hdrlen);
-	skb_pull(skb, iv_len);
-	return 0;
 }
 
 void wfx_rx_cb(struct wfx_vif *wvif,
@@ -73,12 +40,6 @@ void wfx_rx_cb(struct wfx_vif *wvif,
 
 	memset(hdr, 0, sizeof(*hdr));
 
-	// FIXME: Why do we drop these frames?
-	if (!arg->rcpi_rssi &&
-	    (ieee80211_is_probe_resp(frame->frame_control) ||
-	     ieee80211_is_beacon(frame->frame_control)))
-		goto drop;
-
 	if (arg->status == HIF_STATUS_RX_FAIL_MIC)
 		hdr->flag |= RX_FLAG_MMIC_ERROR;
 	else if (arg->status)
@@ -102,24 +63,26 @@ void wfx_rx_cb(struct wfx_vif *wvif,
 		hdr->rate_idx = arg->rxed_rate;
 	}
 
+	if (!arg->rcpi_rssi) {
+		hdr->flag |= RX_FLAG_NO_SIGNAL_VAL;
+		dev_info(wvif->wdev->dev, "received frame without RSSI data\n");
+	}
 	hdr->signal = arg->rcpi_rssi / 2 - 110;
 	hdr->antenna = 0;
 
-	if (arg->rx_flags.encryp) {
-		if (wfx_drop_encrypt_data(wvif->wdev, arg, skb))
-			goto drop;
-		hdr->flag |= RX_FLAG_DECRYPTED | RX_FLAG_IV_STRIPPED;
-		if (arg->rx_flags.encryp == HIF_RI_FLAGS_TKIP_ENCRYPTED)
-			hdr->flag |= RX_FLAG_MMIC_STRIPPED;
-	}
+	if (arg->rx_flags.encryp)
+		hdr->flag |= RX_FLAG_DECRYPTED;
 
-	/* Filter block ACK negotiation: fully controlled by firmware */
+	// Block ack negociation is offloaded by the firmware. However,
+	// re-ordering must be done by the mac80211.
 	if (ieee80211_is_action(frame->frame_control) &&
-	    arg->rx_flags.match_uc_addr &&
-	    mgmt->u.action.category == WLAN_CATEGORY_BACK)
+	    mgmt->u.action.category == WLAN_CATEGORY_BACK &&
+	    skb->len > IEEE80211_MIN_ACTION_SIZE) {
+		wfx_rx_handle_ba(wvif, mgmt);
 		goto drop;
-	ieee80211_rx_irqsafe(wvif->wdev->hw, skb);
+	}
 
+	ieee80211_rx_irqsafe(wvif->wdev->hw, skb);
 	return;
 
 drop:
diff --git a/drivers/staging/wfx/data_tx.c b/drivers/staging/wfx/data_tx.c
index f042ef36b408..3acf4eb0214d 100644
--- a/drivers/staging/wfx/data_tx.c
+++ b/drivers/staging/wfx/data_tx.c
@@ -213,23 +213,6 @@ static bool ieee80211_is_action_back(struct ieee80211_hdr *hdr)
 	return true;
 }
 
-static void wfx_tx_manage_pm(struct wfx_vif *wvif, struct ieee80211_hdr *hdr,
-			     struct wfx_tx_priv *tx_priv,
-			     struct ieee80211_sta *sta)
-{
-	struct wfx_sta_priv *sta_priv;
-	int tid = ieee80211_get_tid(hdr);
-
-	if (sta) {
-		tx_priv->has_sta = true;
-		sta_priv = (struct wfx_sta_priv *)&sta->drv_priv;
-		spin_lock_bh(&sta_priv->lock);
-		sta_priv->buffered[tid]++;
-		ieee80211_sta_set_buffered(sta, tid, true);
-		spin_unlock_bh(&sta_priv->lock);
-	}
-}
-
 static u8 wfx_tx_get_link_id(struct wfx_vif *wvif, struct ieee80211_sta *sta,
 			     struct ieee80211_hdr *hdr)
 {
@@ -407,8 +390,7 @@ static int wfx_tx_inner(struct wfx_vif *wvif, struct ieee80211_sta *sta,
 	req->tx_flags.retry_policy_index = wfx_tx_get_rate_id(wvif, tx_info);
 
 	// Auxiliary operations
-	wfx_tx_manage_pm(wvif, hdr, tx_priv, sta);
-	wfx_tx_queues_put(wvif->wdev, skb);
+	wfx_tx_queues_put(wvif, skb);
 	if (tx_info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM)
 		schedule_work(&wvif->update_tim_work);
 	wfx_bh_request_tx(wvif->wdev);
@@ -436,7 +418,8 @@ void wfx_tx(struct ieee80211_hw *hw, struct ieee80211_tx_control *control,
 		wvif = wvif_iterate(wdev, NULL);
 	if (WARN_ON(!wvif))
 		goto drop;
-	// FIXME: why?
+	// Because of TX_AMPDU_SETUP_IN_HW, mac80211 does not try to send any
+	// BlockAck session management frame. The check below exist just in case.
 	if (ieee80211_is_action_back(hdr)) {
 		dev_info(wdev->dev, "drop BA action\n");
 		goto drop;
@@ -450,37 +433,6 @@ drop:
 	ieee80211_tx_status_irqsafe(wdev->hw, skb);
 }
 
-static struct ieee80211_hdr *wfx_skb_hdr80211(struct sk_buff *skb)
-{
-	struct hif_msg *hif = (struct hif_msg *)skb->data;
-	struct hif_req_tx *req = (struct hif_req_tx *)hif->body;
-
-	return (struct ieee80211_hdr *)(req->frame + req->data_flags.fc_offset);
-}
-
-static void wfx_tx_update_sta(struct wfx_vif *wvif, struct ieee80211_hdr *hdr)
-{
-	int tid = ieee80211_get_tid(hdr);
-	struct wfx_sta_priv *sta_priv;
-	struct ieee80211_sta *sta;
-
-	rcu_read_lock(); // protect sta
-	sta = ieee80211_find_sta(wvif->vif, hdr->addr1);
-	if (sta) {
-		sta_priv = (struct wfx_sta_priv *)&sta->drv_priv;
-		spin_lock_bh(&sta_priv->lock);
-		WARN(!sta_priv->buffered[tid], "inconsistent notification");
-		sta_priv->buffered[tid]--;
-		if (!sta_priv->buffered[tid])
-			ieee80211_sta_set_buffered(sta, tid, false);
-		spin_unlock_bh(&sta_priv->lock);
-	} else {
-		dev_dbg(wvif->wdev->dev, "%s: sta does not exist anymore\n",
-			__func__);
-	}
-	rcu_read_unlock();
-}
-
 static void wfx_skb_dtor(struct wfx_vif *wvif, struct sk_buff *skb)
 {
 	struct hif_msg *hif = (struct hif_msg *)skb->data;
@@ -533,27 +485,29 @@ static void wfx_tx_fill_rates(struct wfx_dev *wdev,
 		dev_dbg(wdev->dev, "%d more retries than expected\n", tx_count);
 }
 
-void wfx_tx_confirm_cb(struct wfx_vif *wvif, const struct hif_cnf_tx *arg)
+void wfx_tx_confirm_cb(struct wfx_dev *wdev, const struct hif_cnf_tx *arg)
 {
 	struct ieee80211_tx_info *tx_info;
 	const struct wfx_tx_priv *tx_priv;
+	struct wfx_vif *wvif;
 	struct sk_buff *skb;
 
-	skb = wfx_pending_get(wvif->wdev, arg->packet_id);
+	skb = wfx_pending_get(wdev, arg->packet_id);
 	if (!skb) {
-		dev_warn(wvif->wdev->dev, "received unknown packet_id (%#.8x) from chip\n",
+		dev_warn(wdev->dev, "received unknown packet_id (%#.8x) from chip\n",
 			 arg->packet_id);
 		return;
 	}
+	wvif = wdev_to_wvif(wdev, ((struct hif_msg *)skb->data)->interface);
+	WARN_ON(!wvif);
+	if (!wvif)
+		return;
 	tx_info = IEEE80211_SKB_CB(skb);
 	tx_priv = wfx_skb_tx_priv(skb);
-	_trace_tx_stats(arg, skb,
-			wfx_pending_get_pkt_us_delay(wvif->wdev, skb));
+	_trace_tx_stats(arg, skb, wfx_pending_get_pkt_us_delay(wdev, skb));
 
 	// You can touch to tx_priv, but don't touch to tx_info->status.
-	wfx_tx_fill_rates(wvif->wdev, tx_info, arg);
-	if (tx_priv->has_sta)
-		wfx_tx_update_sta(wvif, wfx_skb_hdr80211(skb));
+	wfx_tx_fill_rates(wdev, tx_info, arg);
 	skb_trim(skb, skb->len - wfx_tx_get_icv_len(tx_priv->hw_key));
 
 	// From now, you can touch to tx_info->status, but do not touch to
@@ -582,45 +536,58 @@ void wfx_tx_confirm_cb(struct wfx_vif *wvif, const struct hif_cnf_tx *arg)
 	wfx_skb_dtor(wvif, skb);
 }
 
-void wfx_flush(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
-	       u32 queues, bool drop)
+static void wfx_flush_vif(struct wfx_vif *wvif, u32 queues,
+			  struct sk_buff_head *dropped)
 {
-	struct wfx_dev *wdev = hw->priv;
-	struct sk_buff_head dropped;
 	struct wfx_queue *queue;
-	struct wfx_vif *wvif;
-	struct hif_msg *hif;
-	struct sk_buff *skb;
-	int vif_id = -1;
 	int i;
 
-	if (vif)
-		vif_id = ((struct wfx_vif *)vif->drv_priv)->id;
-	skb_queue_head_init(&dropped);
 	for (i = 0; i < IEEE80211_NUM_ACS; i++) {
 		if (!(BIT(i) & queues))
 			continue;
-		queue = &wdev->tx_queue[i];
-		if (drop)
-			wfx_tx_queue_drop(wdev, queue, vif_id, &dropped);
-		if (wdev->chip_frozen)
+		queue = &wvif->tx_queue[i];
+		if (dropped)
+			wfx_tx_queue_drop(wvif, queue, dropped);
+	}
+	if (wvif->wdev->chip_frozen)
+		return;
+	for (i = 0; i < IEEE80211_NUM_ACS; i++) {
+		if (!(BIT(i) & queues))
 			continue;
-		if (wait_event_timeout(wdev->tx_dequeue,
-				       wfx_tx_queue_empty(wdev, queue, vif_id),
+		queue = &wvif->tx_queue[i];
+		if (wait_event_timeout(wvif->wdev->tx_dequeue,
+				       wfx_tx_queue_empty(wvif, queue),
 				       msecs_to_jiffies(1000)) <= 0)
-			dev_warn(wdev->dev,
+			dev_warn(wvif->wdev->dev,
 				 "frames queued while flushing tx queues?");
 	}
+}
+
+void wfx_flush(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+	       u32 queues, bool drop)
+{
+	struct wfx_dev *wdev = hw->priv;
+	struct sk_buff_head dropped;
+	struct wfx_vif *wvif;
+	struct hif_msg *hif;
+	struct sk_buff *skb;
+
+	skb_queue_head_init(&dropped);
+	if (vif) {
+		wvif = (struct wfx_vif *)vif->drv_priv;
+		wfx_flush_vif(wvif, queues, drop ? &dropped : NULL);
+	} else {
+		wvif = NULL;
+		while ((wvif = wvif_iterate(wdev, wvif)) != NULL)
+			wfx_flush_vif(wvif, queues, drop ? &dropped : NULL);
+	}
 	wfx_tx_flush(wdev);
 	if (wdev->chip_frozen)
 		wfx_pending_drop(wdev, &dropped);
 	while ((skb = skb_dequeue(&dropped)) != NULL) {
 		hif = (struct hif_msg *)skb->data;
 		wvif = wdev_to_wvif(wdev, hif->interface);
-		if (wfx_skb_tx_priv(skb)->has_sta)
-			wfx_tx_update_sta(wvif, wfx_skb_hdr80211(skb));
 		ieee80211_tx_info_clear_status(IEEE80211_SKB_CB(skb));
 		wfx_skb_dtor(wvif, skb);
 	}
 }
-
diff --git a/drivers/staging/wfx/data_tx.h b/drivers/staging/wfx/data_tx.h
index 54fff24508fb..cff7b9ff99a9 100644
--- a/drivers/staging/wfx/data_tx.h
+++ b/drivers/staging/wfx/data_tx.h
@@ -36,7 +36,6 @@ struct tx_policy_cache {
 struct wfx_tx_priv {
 	ktime_t xmit_timestamp;
 	struct ieee80211_key_conf *hw_key;
-	bool has_sta;
 } __packed;
 
 void wfx_tx_policy_init(struct wfx_vif *wvif);
@@ -44,7 +43,7 @@ void wfx_tx_policy_upload_work(struct work_struct *work);
 
 void wfx_tx(struct ieee80211_hw *hw, struct ieee80211_tx_control *control,
 	    struct sk_buff *skb);
-void wfx_tx_confirm_cb(struct wfx_vif *wvif, const struct hif_cnf_tx *arg);
+void wfx_tx_confirm_cb(struct wfx_dev *wdev, const struct hif_cnf_tx *arg);
 void wfx_flush(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	       u32 queues, bool drop);
 
diff --git a/drivers/staging/wfx/debug.c b/drivers/staging/wfx/debug.c
index 10d649985696..3f1712b7c919 100644
--- a/drivers/staging/wfx/debug.c
+++ b/drivers/staging/wfx/debug.c
@@ -334,6 +334,28 @@ static const struct file_operations wfx_send_hif_msg_fops = {
 	.read = wfx_send_hif_msg_read,
 };
 
+static int wfx_ps_timeout_set(void *data, u64 val)
+{
+	struct wfx_dev *wdev = (struct wfx_dev *)data;
+	struct wfx_vif *wvif;
+
+	wdev->force_ps_timeout = val;
+	wvif = NULL;
+	while ((wvif = wvif_iterate(wdev, wvif)) != NULL)
+		wfx_update_pm(wvif);
+	return 0;
+}
+
+static int wfx_ps_timeout_get(void *data, u64 *val)
+{
+	struct wfx_dev *wdev = (struct wfx_dev *)data;
+
+	*val = wdev->force_ps_timeout;
+	return 0;
+}
+
+DEFINE_DEBUGFS_ATTRIBUTE(wfx_ps_timeout_fops, wfx_ps_timeout_get, wfx_ps_timeout_set, "%lld\n");
+
 int wfx_debug_init(struct wfx_dev *wdev)
 {
 	struct dentry *d;
@@ -348,6 +370,7 @@ int wfx_debug_init(struct wfx_dev *wdev)
 			    &wfx_burn_slk_key_fops);
 	debugfs_create_file("send_hif_msg", 0600, d, wdev,
 			    &wfx_send_hif_msg_fops);
+	debugfs_create_file("ps_timeout", 0600, d, wdev, &wfx_ps_timeout_fops);
 
 	return 0;
 }
diff --git a/drivers/staging/wfx/fwio.c b/drivers/staging/wfx/fwio.c
index 72bb3d2a9613..22d3b684f04f 100644
--- a/drivers/staging/wfx/fwio.c
+++ b/drivers/staging/wfx/fwio.c
@@ -177,7 +177,7 @@ static int wait_ncp_status(struct wfx_dev *wdev, u32 status)
 static int upload_firmware(struct wfx_dev *wdev, const u8 *data, size_t len)
 {
 	int ret;
-	u32 offs, bytes_done;
+	u32 offs, bytes_done = 0;
 	ktime_t now, start;
 
 	if (len % DNLD_BLOCK_SIZE) {
@@ -188,15 +188,14 @@ static int upload_firmware(struct wfx_dev *wdev, const u8 *data, size_t len)
 	while (offs < len) {
 		start = ktime_get();
 		for (;;) {
-			ret = sram_reg_read(wdev, WFX_DCA_GET, &bytes_done);
-			if (ret < 0)
-				return ret;
 			now = ktime_get();
-			if (offs +
-			    DNLD_BLOCK_SIZE - bytes_done < DNLD_FIFO_SIZE)
+			if (offs + DNLD_BLOCK_SIZE - bytes_done < DNLD_FIFO_SIZE)
 				break;
 			if (ktime_after(now, ktime_add_ms(start, DCA_TIMEOUT)))
 				return -ETIMEDOUT;
+			ret = sram_reg_read(wdev, WFX_DCA_GET, &bytes_done);
+			if (ret < 0)
+				return ret;
 		}
 		if (ktime_compare(now, start))
 			dev_dbg(wdev->dev, "answer after %lldus\n",
@@ -398,10 +397,9 @@ int wfx_init_device(struct wfx_dev *wdev)
 	ret = load_firmware_secure(wdev);
 	if (ret < 0)
 		return ret;
-	ret = config_reg_write_bits(wdev,
-				    CFG_DIRECT_ACCESS_MODE |
-				    CFG_IRQ_ENABLE_DATA |
-				    CFG_IRQ_ENABLE_WRDY,
-				    CFG_IRQ_ENABLE_DATA);
-	return ret;
+	return config_reg_write_bits(wdev,
+				     CFG_DIRECT_ACCESS_MODE |
+				     CFG_IRQ_ENABLE_DATA |
+				     CFG_IRQ_ENABLE_WRDY,
+				     CFG_IRQ_ENABLE_DATA);
 }
diff --git a/drivers/staging/wfx/hif_rx.c b/drivers/staging/wfx/hif_rx.c
index bb156033d1e1..cc7c0cf226ba 100644
--- a/drivers/staging/wfx/hif_rx.c
+++ b/drivers/staging/wfx/hif_rx.c
@@ -63,13 +63,8 @@ static int hif_tx_confirm(struct wfx_dev *wdev,
 			  const struct hif_msg *hif, const void *buf)
 {
 	const struct hif_cnf_tx *body = buf;
-	struct wfx_vif *wvif = wdev_to_wvif(wdev, hif->interface);
-
-	WARN_ON(!wvif);
-	if (!wvif)
-		return -EFAULT;
 
-	wfx_tx_confirm_cb(wvif, body);
+	wfx_tx_confirm_cb(wdev, body);
 	return 0;
 }
 
@@ -77,16 +72,11 @@ static int hif_multi_tx_confirm(struct wfx_dev *wdev,
 				const struct hif_msg *hif, const void *buf)
 {
 	const struct hif_cnf_multi_transmit *body = buf;
-	struct wfx_vif *wvif = wdev_to_wvif(wdev, hif->interface);
 	int i;
 
 	WARN(body->num_tx_confs <= 0, "corrupted message");
-	WARN_ON(!wvif);
-	if (!wvif)
-		return -EFAULT;
-
 	for (i = 0; i < body->num_tx_confs; i++)
-		wfx_tx_confirm_cb(wvif, &body->tx_conf_payload[i]);
+		wfx_tx_confirm_cb(wdev, &body->tx_conf_payload[i]);
 	return 0;
 }
 
@@ -159,7 +149,6 @@ static int hif_event_indication(struct wfx_dev *wdev,
 	struct wfx_vif *wvif = wdev_to_wvif(wdev, hif->interface);
 	const struct hif_ind_event *body = buf;
 	int type = le32_to_cpu(body->event_id);
-	int cause;
 
 	if (!wvif) {
 		dev_warn(wdev->dev, "received event for non-existent vif\n");
@@ -178,13 +167,8 @@ static int hif_event_indication(struct wfx_dev *wdev,
 		dev_dbg(wdev->dev, "ignore BSSREGAINED indication\n");
 		break;
 	case HIF_EVENT_IND_PS_MODE_ERROR:
-		cause = le32_to_cpu(body->event_data.ps_mode_error);
 		dev_warn(wdev->dev, "error while processing power save request: %d\n",
-			 cause);
-		if (cause == HIF_PS_ERROR_AP_NOT_RESP_TO_POLL) {
-			wvif->bss_not_support_ps_poll = true;
-			schedule_work(&wvif->update_pm_work);
-		}
+			 le32_to_cpu(body->event_data.ps_mode_error));
 		break;
 	default:
 		dev_warn(wdev->dev, "unhandled event indication: %.2x\n",
diff --git a/drivers/staging/wfx/hif_tx_mib.c b/drivers/staging/wfx/hif_tx_mib.c
index 1689cb42acc0..05f1e1e98af9 100644
--- a/drivers/staging/wfx/hif_tx_mib.c
+++ b/drivers/staging/wfx/hif_tx_mib.c
@@ -113,7 +113,7 @@ int hif_set_beacon_filter_table(struct wfx_vif *wvif, int tbl_len,
 	if (!val)
 		return -ENOMEM;
 	val->num_of_info_elmts = cpu_to_le32(tbl_len);
-	memcpy(val->ie_table, tbl, tbl_len * sizeof(*tbl));
+	memcpy(val->ie_table, tbl, flex_array_size(val, ie_table, tbl_len));
 	ret = hif_write_mib(wvif->wdev, wvif->id,
 			    HIF_MIB_ID_BEACON_FILTER_TABLE, val, buf_len);
 	kfree(val);
diff --git a/drivers/staging/wfx/main.c b/drivers/staging/wfx/main.c
index 6bd96f476388..11dfa088fc86 100644
--- a/drivers/staging/wfx/main.c
+++ b/drivers/staging/wfx/main.c
@@ -13,7 +13,6 @@
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_net.h>
-#include <linux/gpio.h>
 #include <linux/gpio/consumer.h>
 #include <linux/mmc/sdio_func.h>
 #include <linux/spi/spi.h>
@@ -41,10 +40,6 @@ MODULE_DESCRIPTION("Silicon Labs 802.11 Wireless LAN driver for WFx");
 MODULE_AUTHOR("Jérôme Pouiller <jerome.pouiller@silabs.com>");
 MODULE_LICENSE("GPL");
 
-static int gpio_wakeup = -2;
-module_param(gpio_wakeup, int, 0644);
-MODULE_PARM_DESC(gpio_wakeup, "gpio number for wakeup. -1 for none.");
-
 #define RATETAB_ENT(_rate, _rateid, _flags) { \
 	.bitrate  = (_rate),   \
 	.hw_value = (_rateid), \
@@ -170,38 +165,6 @@ bool wfx_api_older_than(struct wfx_dev *wdev, int major, int minor)
 	return false;
 }
 
-struct gpio_desc *wfx_get_gpio(struct device *dev,
-			       int override, const char *label)
-{
-	struct gpio_desc *ret;
-	char label_buf[256];
-
-	if (override >= 0) {
-		snprintf(label_buf, sizeof(label_buf), "wfx_%s", label);
-		ret = ERR_PTR(devm_gpio_request_one(dev, override,
-						    GPIOF_OUT_INIT_LOW,
-						    label_buf));
-		if (!ret)
-			ret = gpio_to_desc(override);
-	} else if (override == -1) {
-		ret = NULL;
-	} else {
-		ret = devm_gpiod_get(dev, label, GPIOD_OUT_LOW);
-	}
-	if (IS_ERR_OR_NULL(ret)) {
-		if (!ret || PTR_ERR(ret) == -ENOENT)
-			dev_warn(dev, "gpio %s is not defined\n", label);
-		else
-			dev_warn(dev, "error while requesting gpio %s\n",
-				 label);
-		ret = NULL;
-	} else {
-		dev_dbg(dev, "using gpio %d for %s\n",
-			desc_to_gpio(ret), label);
-	}
-	return ret;
-}
-
 /* NOTE: wfx_send_pds() destroy buf */
 int wfx_send_pds(struct wfx_dev *wdev, u8 *buf, size_t len)
 {
@@ -340,7 +303,12 @@ struct wfx_dev *wfx_init_common(struct device *dev,
 	memcpy(&wdev->pdata, pdata, sizeof(*pdata));
 	of_property_read_string(dev->of_node, "config-file",
 				&wdev->pdata.file_pds);
-	wdev->pdata.gpio_wakeup = wfx_get_gpio(dev, gpio_wakeup, "wakeup");
+	wdev->pdata.gpio_wakeup = devm_gpiod_get_optional(dev, "wakeup",
+							  GPIOD_OUT_LOW);
+	if (IS_ERR(wdev->pdata.gpio_wakeup))
+		return ERR_CAST(wdev->pdata.gpio_wakeup);
+	if (wdev->pdata.gpio_wakeup)
+		gpiod_set_consumer_name(wdev->pdata.gpio_wakeup, "wfx wakeup");
 	wfx_sl_fill_pdata(dev, &wdev->pdata);
 
 	mutex_init(&wdev->conf_mutex);
@@ -349,8 +317,10 @@ struct wfx_dev *wfx_init_common(struct device *dev,
 	init_completion(&wdev->firmware_ready);
 	INIT_DELAYED_WORK(&wdev->cooling_timeout_work,
 			  wfx_cooling_timeout_work);
+	skb_queue_head_init(&wdev->tx_pending);
+	init_waitqueue_head(&wdev->tx_dequeue);
 	wfx_init_hif_cmd(&wdev->hif_cmd);
-	wfx_tx_queues_init(wdev);
+	wdev->force_ps_timeout = -1;
 
 	if (devm_add_action_or_reset(dev, wfx_free_common, wdev))
 		return NULL;
@@ -442,8 +412,7 @@ int wfx_probe(struct wfx_dev *wdev)
 	wdev->pdata.gpio_wakeup = gpio_saved;
 	if (wdev->pdata.gpio_wakeup) {
 		dev_dbg(wdev->dev,
-			"enable 'quiescent' power mode with gpio %d and PDS file %s\n",
-			desc_to_gpio(wdev->pdata.gpio_wakeup),
+			"enable 'quiescent' power mode with wakeup GPIO and PDS file %s\n",
 			wdev->pdata.file_pds);
 		gpiod_set_value_cansleep(wdev->pdata.gpio_wakeup, 1);
 		control_reg_write(wdev, 0);
diff --git a/drivers/staging/wfx/main.h b/drivers/staging/wfx/main.h
index f832ce409fda..c59d375dd3ad 100644
--- a/drivers/staging/wfx/main.h
+++ b/drivers/staging/wfx/main.h
@@ -38,8 +38,6 @@ struct wfx_dev *wfx_init_common(struct device *dev,
 int wfx_probe(struct wfx_dev *wdev);
 void wfx_release(struct wfx_dev *wdev);
 
-struct gpio_desc *wfx_get_gpio(struct device *dev, int override,
-			       const char *label);
 bool wfx_api_older_than(struct wfx_dev *wdev, int major, int minor);
 int wfx_send_pds(struct wfx_dev *wdev, u8 *buf, size_t len);
 
diff --git a/drivers/staging/wfx/queue.c b/drivers/staging/wfx/queue.c
index 93ea2b72febd..6e3159165143 100644
--- a/drivers/staging/wfx/queue.c
+++ b/drivers/staging/wfx/queue.c
@@ -12,6 +12,7 @@
 #include "wfx.h"
 #include "sta.h"
 #include "data_tx.h"
+#include "traces.h"
 
 void wfx_tx_lock(struct wfx_dev *wdev)
 {
@@ -57,84 +58,57 @@ void wfx_tx_lock_flush(struct wfx_dev *wdev)
 	wfx_tx_flush(wdev);
 }
 
-void wfx_tx_queues_init(struct wfx_dev *wdev)
+void wfx_tx_queues_init(struct wfx_vif *wvif)
 {
 	int i;
 
-	skb_queue_head_init(&wdev->tx_pending);
-	init_waitqueue_head(&wdev->tx_dequeue);
 	for (i = 0; i < IEEE80211_NUM_ACS; ++i) {
-		skb_queue_head_init(&wdev->tx_queue[i].normal);
-		skb_queue_head_init(&wdev->tx_queue[i].cab);
+		skb_queue_head_init(&wvif->tx_queue[i].normal);
+		skb_queue_head_init(&wvif->tx_queue[i].cab);
 	}
 }
 
-void wfx_tx_queues_check_empty(struct wfx_dev *wdev)
+void wfx_tx_queues_check_empty(struct wfx_vif *wvif)
 {
 	int i;
 
-	WARN_ON(!skb_queue_empty_lockless(&wdev->tx_pending));
 	for (i = 0; i < IEEE80211_NUM_ACS; ++i) {
-		WARN_ON(atomic_read(&wdev->tx_queue[i].pending_frames));
-		WARN_ON(!skb_queue_empty_lockless(&wdev->tx_queue[i].normal));
-		WARN_ON(!skb_queue_empty_lockless(&wdev->tx_queue[i].cab));
+		WARN_ON(atomic_read(&wvif->tx_queue[i].pending_frames));
+		WARN_ON(!skb_queue_empty_lockless(&wvif->tx_queue[i].normal));
+		WARN_ON(!skb_queue_empty_lockless(&wvif->tx_queue[i].cab));
 	}
 }
 
-static bool __wfx_tx_queue_empty(struct wfx_dev *wdev,
-				 struct sk_buff_head *skb_queue, int vif_id)
+bool wfx_tx_queue_empty(struct wfx_vif *wvif, struct wfx_queue *queue)
 {
-	struct hif_msg *hif_msg;
-	struct sk_buff *skb;
-
-	spin_lock_bh(&skb_queue->lock);
-	skb_queue_walk(skb_queue, skb) {
-		hif_msg = (struct hif_msg *)skb->data;
-		if (vif_id < 0 || hif_msg->interface == vif_id) {
-			spin_unlock_bh(&skb_queue->lock);
-			return false;
-		}
-	}
-	spin_unlock_bh(&skb_queue->lock);
-	return true;
-}
-
-bool wfx_tx_queue_empty(struct wfx_dev *wdev,
-			struct wfx_queue *queue, int vif_id)
-{
-	return __wfx_tx_queue_empty(wdev, &queue->normal, vif_id) &&
-	       __wfx_tx_queue_empty(wdev, &queue->cab, vif_id);
+	return skb_queue_empty(&queue->normal) && skb_queue_empty(&queue->cab);
 }
 
-static void __wfx_tx_queue_drop(struct wfx_dev *wdev,
-				struct sk_buff_head *skb_queue, int vif_id,
+static void __wfx_tx_queue_drop(struct wfx_vif *wvif,
+				struct sk_buff_head *skb_queue,
 				struct sk_buff_head *dropped)
 {
 	struct sk_buff *skb, *tmp;
-	struct hif_msg *hif_msg;
 
 	spin_lock_bh(&skb_queue->lock);
 	skb_queue_walk_safe(skb_queue, skb, tmp) {
-		hif_msg = (struct hif_msg *)skb->data;
-		if (vif_id < 0 || hif_msg->interface == vif_id) {
-			__skb_unlink(skb, skb_queue);
-			skb_queue_head(dropped, skb);
-		}
+		__skb_unlink(skb, skb_queue);
+		skb_queue_head(dropped, skb);
 	}
 	spin_unlock_bh(&skb_queue->lock);
 }
 
-void wfx_tx_queue_drop(struct wfx_dev *wdev, struct wfx_queue *queue,
-		       int vif_id, struct sk_buff_head *dropped)
+void wfx_tx_queue_drop(struct wfx_vif *wvif, struct wfx_queue *queue,
+		       struct sk_buff_head *dropped)
 {
-	__wfx_tx_queue_drop(wdev, &queue->cab, vif_id, dropped);
-	__wfx_tx_queue_drop(wdev, &queue->normal, vif_id, dropped);
-	wake_up(&wdev->tx_dequeue);
+	__wfx_tx_queue_drop(wvif, &queue->cab, dropped);
+	__wfx_tx_queue_drop(wvif, &queue->normal, dropped);
+	wake_up(&wvif->wdev->tx_dequeue);
 }
 
-void wfx_tx_queues_put(struct wfx_dev *wdev, struct sk_buff *skb)
+void wfx_tx_queues_put(struct wfx_vif *wvif, struct sk_buff *skb)
 {
-	struct wfx_queue *queue = &wdev->tx_queue[skb_get_queue_mapping(skb)];
+	struct wfx_queue *queue = &wvif->tx_queue[skb_get_queue_mapping(skb)];
 	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
 
 	if (tx_info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM)
@@ -146,15 +120,21 @@ void wfx_tx_queues_put(struct wfx_dev *wdev, struct sk_buff *skb)
 void wfx_pending_drop(struct wfx_dev *wdev, struct sk_buff_head *dropped)
 {
 	struct wfx_queue *queue;
+	struct wfx_vif *wvif;
+	struct hif_msg *hif;
 	struct sk_buff *skb;
 
 	WARN(!wdev->chip_frozen, "%s should only be used to recover a frozen device",
 	     __func__);
 	while ((skb = skb_dequeue(&wdev->tx_pending)) != NULL) {
-		queue = &wdev->tx_queue[skb_get_queue_mapping(skb)];
-		WARN_ON(skb_get_queue_mapping(skb) > 3);
-		WARN_ON(!atomic_read(&queue->pending_frames));
-		atomic_dec(&queue->pending_frames);
+		hif = (struct hif_msg *)skb->data;
+		wvif = wdev_to_wvif(wdev, hif->interface);
+		if (wvif) {
+			queue = &wvif->tx_queue[skb_get_queue_mapping(skb)];
+			WARN_ON(skb_get_queue_mapping(skb) > 3);
+			WARN_ON(!atomic_read(&queue->pending_frames));
+			atomic_dec(&queue->pending_frames);
+		}
 		skb_queue_head(dropped, skb);
 	}
 }
@@ -163,20 +143,26 @@ struct sk_buff *wfx_pending_get(struct wfx_dev *wdev, u32 packet_id)
 {
 	struct wfx_queue *queue;
 	struct hif_req_tx *req;
+	struct wfx_vif *wvif;
+	struct hif_msg *hif;
 	struct sk_buff *skb;
 
 	spin_lock_bh(&wdev->tx_pending.lock);
 	skb_queue_walk(&wdev->tx_pending, skb) {
-		req = wfx_skb_txreq(skb);
-		if (req->packet_id == packet_id) {
-			spin_unlock_bh(&wdev->tx_pending.lock);
-			queue = &wdev->tx_queue[skb_get_queue_mapping(skb)];
+		hif = (struct hif_msg *)skb->data;
+		req = (struct hif_req_tx *)hif->body;
+		if (req->packet_id != packet_id)
+			continue;
+		spin_unlock_bh(&wdev->tx_pending.lock);
+		wvif = wdev_to_wvif(wdev, hif->interface);
+		if (wvif) {
+			queue = &wvif->tx_queue[skb_get_queue_mapping(skb)];
 			WARN_ON(skb_get_queue_mapping(skb) > 3);
 			WARN_ON(!atomic_read(&queue->pending_frames));
 			atomic_dec(&queue->pending_frames);
-			skb_unlink(skb, &wdev->tx_pending);
-			return skb;
 		}
+		skb_unlink(skb, &wdev->tx_pending);
+		return skb;
 	}
 	spin_unlock_bh(&wdev->tx_pending.lock);
 	WARN(1, "cannot find packet in pending queue");
@@ -221,7 +207,6 @@ unsigned int wfx_pending_get_pkt_us_delay(struct wfx_dev *wdev,
 
 bool wfx_tx_queues_has_cab(struct wfx_vif *wvif)
 {
-	struct wfx_dev *wdev = wvif->wdev;
 	int i;
 
 	if (wvif->vif->type != NL80211_IFTYPE_AP)
@@ -229,33 +214,39 @@ bool wfx_tx_queues_has_cab(struct wfx_vif *wvif)
 	for (i = 0; i < IEEE80211_NUM_ACS; ++i)
 		// Note: since only AP can have mcast frames in queue and only
 		// one vif can be AP, all queued frames has same interface id
-		if (!skb_queue_empty_lockless(&wdev->tx_queue[i].cab))
+		if (!skb_queue_empty_lockless(&wvif->tx_queue[i].cab))
 			return true;
 	return false;
 }
 
 static struct sk_buff *wfx_tx_queues_get_skb(struct wfx_dev *wdev)
 {
-	struct wfx_queue *sorted_queues[IEEE80211_NUM_ACS];
+	struct wfx_queue *queues[IEEE80211_NUM_ACS * ARRAY_SIZE(wdev->vif)];
+	int i, j, num_queues = 0;
 	struct wfx_vif *wvif;
 	struct hif_msg *hif;
 	struct sk_buff *skb;
-	int i, j;
-
-	// bubble sort
-	for (i = 0; i < IEEE80211_NUM_ACS; i++) {
-		sorted_queues[i] = &wdev->tx_queue[i];
-		for (j = i; j > 0; j--)
-			if (atomic_read(&sorted_queues[j]->pending_frames) <
-			    atomic_read(&sorted_queues[j - 1]->pending_frames))
-				swap(sorted_queues[j - 1], sorted_queues[j]);
+
+	// sort the queues
+	wvif = NULL;
+	while ((wvif = wvif_iterate(wdev, wvif)) != NULL) {
+		for (i = 0; i < IEEE80211_NUM_ACS; i++) {
+			WARN_ON(num_queues >= ARRAY_SIZE(queues));
+			queues[num_queues] = &wvif->tx_queue[i];
+			for (j = num_queues; j > 0; j--)
+				if (atomic_read(&queues[j]->pending_frames) <
+				    atomic_read(&queues[j - 1]->pending_frames))
+					swap(queues[j - 1], queues[j]);
+			num_queues++;
+		}
 	}
+
 	wvif = NULL;
 	while ((wvif = wvif_iterate(wdev, wvif)) != NULL) {
 		if (!wvif->after_dtim_tx_allowed)
 			continue;
-		for (i = 0; i < IEEE80211_NUM_ACS; i++) {
-			skb = skb_dequeue(&sorted_queues[i]->cab);
+		for (i = 0; i < num_queues; i++) {
+			skb = skb_dequeue(&queues[i]->cab);
 			if (!skb)
 				continue;
 			// Note: since only AP can have mcast frames in queue
@@ -263,21 +254,22 @@ static struct sk_buff *wfx_tx_queues_get_skb(struct wfx_dev *wdev)
 			// same interface id
 			hif = (struct hif_msg *)skb->data;
 			WARN_ON(hif->interface != wvif->id);
-			WARN_ON(sorted_queues[i] !=
-				&wdev->tx_queue[skb_get_queue_mapping(skb)]);
-			atomic_inc(&sorted_queues[i]->pending_frames);
+			WARN_ON(queues[i] !=
+				&wvif->tx_queue[skb_get_queue_mapping(skb)]);
+			atomic_inc(&queues[i]->pending_frames);
+			trace_queues_stats(wdev, queues[i]);
 			return skb;
 		}
 		// No more multicast to sent
 		wvif->after_dtim_tx_allowed = false;
 		schedule_work(&wvif->update_tim_work);
 	}
-	for (i = 0; i < IEEE80211_NUM_ACS; i++) {
-		skb = skb_dequeue(&sorted_queues[i]->normal);
+
+	for (i = 0; i < num_queues; i++) {
+		skb = skb_dequeue(&queues[i]->normal);
 		if (skb) {
-			WARN_ON(sorted_queues[i] !=
-				&wdev->tx_queue[skb_get_queue_mapping(skb)]);
-			atomic_inc(&sorted_queues[i]->pending_frames);
+			atomic_inc(&queues[i]->pending_frames);
+			trace_queues_stats(wdev, queues[i]);
 			return skb;
 		}
 	}
diff --git a/drivers/staging/wfx/queue.h b/drivers/staging/wfx/queue.h
index 0c3b7244498e..22d7c936907f 100644
--- a/drivers/staging/wfx/queue.h
+++ b/drivers/staging/wfx/queue.h
@@ -25,16 +25,15 @@ void wfx_tx_unlock(struct wfx_dev *wdev);
 void wfx_tx_flush(struct wfx_dev *wdev);
 void wfx_tx_lock_flush(struct wfx_dev *wdev);
 
-void wfx_tx_queues_init(struct wfx_dev *wdev);
-void wfx_tx_queues_check_empty(struct wfx_dev *wdev);
+void wfx_tx_queues_init(struct wfx_vif *wvif);
+void wfx_tx_queues_check_empty(struct wfx_vif *wvif);
 bool wfx_tx_queues_has_cab(struct wfx_vif *wvif);
-void wfx_tx_queues_put(struct wfx_dev *wdev, struct sk_buff *skb);
+void wfx_tx_queues_put(struct wfx_vif *wvif, struct sk_buff *skb);
 struct hif_msg *wfx_tx_queues_get(struct wfx_dev *wdev);
 
-bool wfx_tx_queue_empty(struct wfx_dev *wdev, struct wfx_queue *queue,
-			int vif_id);
-void wfx_tx_queue_drop(struct wfx_dev *wdev, struct wfx_queue *queue,
-		       int vif_id, struct sk_buff_head *dropped);
+bool wfx_tx_queue_empty(struct wfx_vif *wvif, struct wfx_queue *queue);
+void wfx_tx_queue_drop(struct wfx_vif *wvif, struct wfx_queue *queue,
+		       struct sk_buff_head *dropped);
 
 struct sk_buff *wfx_pending_get(struct wfx_dev *wdev, u32 packet_id);
 void wfx_pending_drop(struct wfx_dev *wdev, struct sk_buff_head *dropped);
diff --git a/drivers/staging/wfx/sta.c b/drivers/staging/wfx/sta.c
index 12e8a5b638f1..4e30ab17a93d 100644
--- a/drivers/staging/wfx/sta.c
+++ b/drivers/staging/wfx/sta.c
@@ -200,7 +200,7 @@ void wfx_configure_filter(struct ieee80211_hw *hw, unsigned int changed_flags,
 	mutex_unlock(&wdev->conf_mutex);
 }
 
-int wfx_get_ps_timeout(struct wfx_vif *wvif, bool *enable_ps)
+static int wfx_get_ps_timeout(struct wfx_vif *wvif, bool *enable_ps)
 {
 	struct ieee80211_channel *chan0 = NULL, *chan1 = NULL;
 	struct ieee80211_conf *conf = &wvif->wdev->hw->conf;
@@ -217,14 +217,18 @@ int wfx_get_ps_timeout(struct wfx_vif *wvif, bool *enable_ps)
 		// are differents.
 		if (enable_ps)
 			*enable_ps = true;
-		if (wvif->bss_not_support_ps_poll)
-			return 30;
-		else
+		if (wvif->wdev->force_ps_timeout > -1)
+			return wvif->wdev->force_ps_timeout;
+		else if (wfx_api_older_than(wvif->wdev, 3, 2))
 			return 0;
+		else
+			return 30;
 	}
 	if (enable_ps)
 		*enable_ps = wvif->vif->bss_conf.ps;
-	if (wvif->vif->bss_conf.assoc && wvif->vif->bss_conf.ps)
+	if (wvif->wdev->force_ps_timeout > -1)
+		return wvif->wdev->force_ps_timeout;
+	else if (wvif->vif->bss_conf.assoc && wvif->vif->bss_conf.ps)
 		return conf->dynamic_ps_timeout;
 	else
 		return -1;
@@ -251,14 +255,6 @@ int wfx_update_pm(struct wfx_vif *wvif)
 	return hif_set_pm(wvif, ps, ps_timeout);
 }
 
-static void wfx_update_pm_work(struct work_struct *work)
-{
-	struct wfx_vif *wvif = container_of(work, struct wfx_vif,
-					    update_pm_work);
-
-	wfx_update_pm(wvif);
-}
-
 int wfx_conf_tx(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 		   u16 queue, const struct ieee80211_tx_queue_params *params)
 {
@@ -368,7 +364,6 @@ void wfx_reset(struct wfx_vif *wvif)
 		hif_set_block_ack_policy(wvif, 0xFF, 0xFF);
 	wfx_tx_unlock(wdev);
 	wvif->join_in_progress = false;
-	wvif->bss_not_support_ps_poll = false;
 	cancel_delayed_work_sync(&wvif->beacon_loss_work);
 	wvif =  NULL;
 	while ((wvif = wvif_iterate(wdev, wvif)) != NULL)
@@ -430,7 +425,6 @@ int wfx_sta_add(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
 	struct wfx_sta_priv *sta_priv = (struct wfx_sta_priv *)&sta->drv_priv;
 
-	spin_lock_init(&sta_priv->lock);
 	sta_priv->vif_id = wvif->id;
 
 	// In station mode, the firmware interprets new link-id as a TDLS peer.
@@ -450,14 +444,7 @@ int wfx_sta_remove(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 {
 	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
 	struct wfx_sta_priv *sta_priv = (struct wfx_sta_priv *)&sta->drv_priv;
-	int i;
 
-	for (i = 0; i < ARRAY_SIZE(sta_priv->buffered); i++)
-		if (sta_priv->buffered[i])
-			// Not an error if paired with trace in
-			// wfx_tx_update_sta()
-			dev_dbg(wvif->wdev->dev, "release station while %d pending frame on queue %d",
-				sta_priv->buffered[i], i);
 	// See note in wfx_sta_add()
 	if (!sta_priv->link_id)
 		return 0;
@@ -794,7 +781,6 @@ int wfx_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 
 	init_completion(&wvif->set_pm_mode_complete);
 	complete(&wvif->set_pm_mode_complete);
-	INIT_WORK(&wvif->update_pm_work, wfx_update_pm_work);
 	INIT_WORK(&wvif->tx_policy_upload_work, wfx_tx_policy_upload_work);
 
 	mutex_init(&wvif->scan_lock);
@@ -805,6 +791,7 @@ int wfx_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 
 	hif_set_macaddr(wvif, vif->addr);
 
+	wfx_tx_queues_init(wvif);
 	wfx_tx_policy_init(wvif);
 	wvif = NULL;
 	while ((wvif = wvif_iterate(wdev, wvif)) != NULL) {
@@ -823,6 +810,7 @@ void wfx_remove_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
 
 	wait_for_completion_timeout(&wvif->set_pm_mode_complete, msecs_to_jiffies(300));
+	wfx_tx_queues_check_empty(wvif);
 
 	mutex_lock(&wdev->conf_mutex);
 	WARN(wvif->link_id_map != 1, "corrupted state");
@@ -855,5 +843,5 @@ void wfx_stop(struct ieee80211_hw *hw)
 {
 	struct wfx_dev *wdev = hw->priv;
 
-	wfx_tx_queues_check_empty(wdev);
+	WARN_ON(!skb_queue_empty_lockless(&wdev->tx_pending));
 }
diff --git a/drivers/staging/wfx/sta.h b/drivers/staging/wfx/sta.h
index 8a20ad9ae017..6b15a64ac9e2 100644
--- a/drivers/staging/wfx/sta.h
+++ b/drivers/staging/wfx/sta.h
@@ -16,9 +16,6 @@ struct wfx_vif;
 struct wfx_sta_priv {
 	int link_id;
 	int vif_id;
-	int buffered[IEEE80211_NUM_TIDS];
-	// Ensure atomicity of "buffered" and calls to ieee80211_sta_set_buffered()
-	spinlock_t lock;
 };
 
 // mac80211 interface
@@ -69,6 +66,7 @@ void wfx_cooling_timeout_work(struct work_struct *work);
 void wfx_suspend_hot_dev(struct wfx_dev *wdev, enum sta_notify_cmd cmd);
 void wfx_suspend_resume_mc(struct wfx_vif *wvif, enum sta_notify_cmd cmd);
 void wfx_event_report_rssi(struct wfx_vif *wvif, u8 raw_rcpi_rssi);
+int wfx_update_pm(struct wfx_vif *wvif);
 
 // Other Helpers
 void wfx_reset(struct wfx_vif *wvif);
diff --git a/drivers/staging/wfx/traces.h b/drivers/staging/wfx/traces.h
index 0b6fbd518638..d376db2f1891 100644
--- a/drivers/staging/wfx/traces.h
+++ b/drivers/staging/wfx/traces.h
@@ -439,6 +439,57 @@ TRACE_EVENT(tx_stats,
 );
 #define _trace_tx_stats(tx_cnf, skb, delay) trace_tx_stats(tx_cnf, skb, delay)
 
+TRACE_EVENT(queues_stats,
+	TP_PROTO(struct wfx_dev *wdev, const struct wfx_queue *elected_queue),
+	TP_ARGS(wdev, elected_queue),
+	TP_STRUCT__entry(
+		__field(int, vif_id)
+		__field(int, queue_id)
+		__array(int, hw, IEEE80211_NUM_ACS * 2)
+		__array(int, drv, IEEE80211_NUM_ACS * 2)
+		__array(int, cab, IEEE80211_NUM_ACS * 2)
+	),
+	TP_fast_assign(
+		const struct wfx_queue *queue;
+		struct wfx_vif *wvif;
+		int i, j;
+
+		for (j = 0; j < IEEE80211_NUM_ACS * 2; j++) {
+			__entry->hw[j] = -1;
+			__entry->drv[j] = -1;
+			__entry->cab[j] = -1;
+		}
+		__entry->vif_id = -1;
+		__entry->queue_id = -1;
+		wvif = NULL;
+		while ((wvif = wvif_iterate(wdev, wvif)) != NULL) {
+			for (i = 0; i < IEEE80211_NUM_ACS; i++) {
+				j = wvif->id * IEEE80211_NUM_ACS + i;
+				WARN_ON(j >= IEEE80211_NUM_ACS * 2);
+				queue = &wvif->tx_queue[i];
+				__entry->hw[j] = atomic_read(&queue->pending_frames);
+				__entry->drv[j] = skb_queue_len(&queue->normal);
+				__entry->cab[j] = skb_queue_len(&queue->cab);
+				if (queue == elected_queue) {
+					__entry->vif_id = wvif->id;
+					__entry->queue_id = i;
+				}
+			}
+		}
+	),
+	TP_printk("got skb from %d/%d, pend. hw/norm/cab: [ %d/%d/%d %d/%d/%d %d/%d/%d %d/%d/%d ] [ %d/%d/%d %d/%d/%d %d/%d/%d %d/%d/%d ]",
+		__entry->vif_id, __entry->queue_id,
+		__entry->hw[0], __entry->drv[0], __entry->cab[0],
+		__entry->hw[1], __entry->drv[1], __entry->cab[1],
+		__entry->hw[2], __entry->drv[2], __entry->cab[2],
+		__entry->hw[3], __entry->drv[3], __entry->cab[3],
+		__entry->hw[4], __entry->drv[4], __entry->cab[4],
+		__entry->hw[5], __entry->drv[5], __entry->cab[5],
+		__entry->hw[6], __entry->drv[6], __entry->cab[6],
+		__entry->hw[7], __entry->drv[7], __entry->cab[7]
+	)
+);
+
 #endif
 
 /* This part must be outside protection */
diff --git a/drivers/staging/wfx/wfx.h b/drivers/staging/wfx/wfx.h
index 73e216733ce4..38e24d7f72f2 100644
--- a/drivers/staging/wfx/wfx.h
+++ b/drivers/staging/wfx/wfx.h
@@ -48,7 +48,6 @@ struct wfx_dev {
 	struct mutex		conf_mutex;
 
 	struct wfx_hif_cmd	hif_cmd;
-	struct wfx_queue	tx_queue[4];
 	struct sk_buff_head	tx_pending;
 	wait_queue_head_t	tx_dequeue;
 	atomic_t		tx_lock;
@@ -60,6 +59,7 @@ struct wfx_dev {
 	struct mutex		rx_stats_lock;
 	struct hif_tx_power_loop_info tx_power_loop_info;
 	struct mutex		tx_power_loop_info_lock;
+	int			force_ps_timeout;
 };
 
 struct wfx_vif {
@@ -75,6 +75,7 @@ struct wfx_vif {
 
 	struct delayed_work	beacon_loss_work;
 
+	struct wfx_queue	tx_queue[4];
 	struct tx_policy_cache	tx_policy_cache;
 	struct work_struct	tx_policy_upload_work;
 
@@ -92,8 +93,6 @@ struct wfx_vif {
 	bool			scan_abort;
 	struct ieee80211_scan_request *scan_req;
 
-	bool			bss_not_support_ps_poll;
-	struct work_struct	update_pm_work;
 	struct completion	set_pm_mode_complete;
 };