1 files changed, 296 insertions, 575 deletions

diff --git a/drivers/staging/wfx/sta.c b/drivers/staging/wfx/sta.c
index 9d430346a58b..12e8a5b638f1 100644
--- a/drivers/staging/wfx/sta.c
+++ b/drivers/staging/wfx/sta.c
@@ -5,6 +5,7 @@
  * Copyright (c) 2017-2019, Silicon Laboratories, Inc.
  * Copyright (c) 2010, ST-Ericsson
  */
+#include <linux/etherdevice.h>
 #include <net/mac80211.h>
 
 #include "sta.h"
@@ -37,117 +38,32 @@ u32 wfx_rate_mask_to_hw(struct wfx_dev *wdev, u32 rates)
 	return ret;
 }
 
-static void __wfx_free_event_queue(struct list_head *list)
+void wfx_cooling_timeout_work(struct work_struct *work)
 {
-	struct wfx_hif_event *event, *tmp;
+	struct wfx_dev *wdev = container_of(to_delayed_work(work),
+					    struct wfx_dev,
+					    cooling_timeout_work);
 
-	list_for_each_entry_safe(event, tmp, list, link) {
-		list_del(&event->link);
-		kfree(event);
-	}
-}
-
-static void wfx_free_event_queue(struct wfx_vif *wvif)
-{
-	LIST_HEAD(list);
-
-	spin_lock(&wvif->event_queue_lock);
-	list_splice_init(&wvif->event_queue, &list);
-	spin_unlock(&wvif->event_queue_lock);
-
-	__wfx_free_event_queue(&list);
+	wdev->chip_frozen = true;
+	wfx_tx_unlock(wdev);
 }
 
-void wfx_cqm_bssloss_sm(struct wfx_vif *wvif, int init, int good, int bad)
+void wfx_suspend_hot_dev(struct wfx_dev *wdev, enum sta_notify_cmd cmd)
 {
-	int tx = 0;
-
-	mutex_lock(&wvif->bss_loss_lock);
-	cancel_work_sync(&wvif->bss_params_work);
-
-	if (init) {
-		schedule_delayed_work(&wvif->bss_loss_work, HZ);
-		wvif->bss_loss_state = 0;
-
-		if (!atomic_read(&wvif->wdev->tx_lock))
-			tx = 1;
-	} else if (good) {
-		cancel_delayed_work_sync(&wvif->bss_loss_work);
-		wvif->bss_loss_state = 0;
-		schedule_work(&wvif->bss_params_work);
-	} else if (bad) {
-		/* FIXME Should we just keep going until we time out? */
-		if (wvif->bss_loss_state < 3)
-			tx = 1;
+	if (cmd == STA_NOTIFY_AWAKE) {
+		// Device recover normal temperature
+		if (cancel_delayed_work(&wdev->cooling_timeout_work))
+			wfx_tx_unlock(wdev);
 	} else {
-		cancel_delayed_work_sync(&wvif->bss_loss_work);
-		wvif->bss_loss_state = 0;
-	}
-
-	/* Spit out a NULL packet to our AP if necessary */
-	// FIXME: call ieee80211_beacon_loss/ieee80211_connection_loss instead
-	if (tx) {
-		struct sk_buff *skb;
-		struct ieee80211_hdr *hdr;
-		struct ieee80211_tx_control control = { };
-
-		wvif->bss_loss_state++;
-
-		skb = ieee80211_nullfunc_get(wvif->wdev->hw, wvif->vif, false);
-		if (!skb)
-			goto end;
-		hdr = (struct ieee80211_hdr *)skb->data;
-		memset(IEEE80211_SKB_CB(skb), 0,
-		       sizeof(*IEEE80211_SKB_CB(skb)));
-		IEEE80211_SKB_CB(skb)->control.vif = wvif->vif;
-		IEEE80211_SKB_CB(skb)->driver_rates[0].idx = 0;
-		IEEE80211_SKB_CB(skb)->driver_rates[0].count = 1;
-		IEEE80211_SKB_CB(skb)->driver_rates[1].idx = -1;
-		rcu_read_lock(); // protect control.sta
-		control.sta = ieee80211_find_sta(wvif->vif, hdr->addr1);
-		wfx_tx(wvif->wdev->hw, &control, skb);
-		rcu_read_unlock();
+		// Device is too hot
+		schedule_delayed_work(&wdev->cooling_timeout_work, 10 * HZ);
+		wfx_tx_lock(wdev);
 	}
-end:
-	mutex_unlock(&wvif->bss_loss_lock);
-}
-
-int wfx_fwd_probe_req(struct wfx_vif *wvif, bool enable)
-{
-	wvif->fwd_probe_req = enable;
-	return hif_set_rx_filter(wvif, wvif->filter_bssid,
-				 wvif->fwd_probe_req);
-}
-
-static int wfx_set_mcast_filter(struct wfx_vif *wvif,
-				    struct wfx_grp_addr_table *fp)
-{
-	int i;
-
-	// Temporary workaround for filters
-	return hif_set_data_filtering(wvif, false, true);
-
-	if (!fp->enable)
-		return hif_set_data_filtering(wvif, false, true);
-
-	for (i = 0; i < fp->num_addresses; i++)
-		hif_set_mac_addr_condition(wvif, i, fp->address_list[i]);
-	hif_set_uc_mc_bc_condition(wvif, 0,
-				   HIF_FILTER_UNICAST | HIF_FILTER_BROADCAST);
-	hif_set_config_data_filter(wvif, true, 0, BIT(1),
-				   BIT(fp->num_addresses) - 1);
-	hif_set_data_filtering(wvif, true, true);
-
-	return 0;
 }
 
-void wfx_update_filtering(struct wfx_vif *wvif)
+static void wfx_filter_beacon(struct wfx_vif *wvif, bool filter_beacon)
 {
-	int ret;
-	int bf_enable;
-	int bf_count;
-	int n_filter_ies;
-	struct hif_ie_table_entry filter_ies[] = {
+	const struct hif_ie_table_entry filter_ies[] = {
 		{
 			.ie_id        = WLAN_EID_VENDOR_SPECIFIC,
 			.has_changed  = 1,
@@ -167,40 +83,33 @@ void wfx_update_filtering(struct wfx_vif *wvif)
 		}
 	};
 
-	if (wvif->state == WFX_STATE_PASSIVE)
-		return;
-
-	if (wvif->disable_beacon_filter) {
-		bf_enable = 0;
-		bf_count = 1;
-		n_filter_ies = 0;
-	} else if (wvif->vif->type != NL80211_IFTYPE_STATION) {
-		bf_enable = HIF_BEACON_FILTER_ENABLE | HIF_BEACON_FILTER_AUTO_ERP;
-		bf_count = 0;
-		n_filter_ies = 2;
+	if (!filter_beacon) {
+		hif_beacon_filter_control(wvif, 0, 1);
 	} else {
-		bf_enable = HIF_BEACON_FILTER_ENABLE;
-		bf_count = 0;
-		n_filter_ies = 3;
+		hif_set_beacon_filter_table(wvif, 3, filter_ies);
+		hif_beacon_filter_control(wvif, HIF_BEACON_FILTER_ENABLE, 0);
 	}
-
-	ret = hif_set_rx_filter(wvif, wvif->filter_bssid, wvif->fwd_probe_req);
-	if (!ret)
-		ret = hif_set_beacon_filter_table(wvif, n_filter_ies, filter_ies);
-	if (!ret)
-		ret = hif_beacon_filter_control(wvif, bf_enable, bf_count);
-	if (!ret)
-		ret = wfx_set_mcast_filter(wvif, &wvif->mcast_filter);
-	if (ret)
-		dev_err(wvif->wdev->dev, "update filtering failed: %d\n", ret);
 }
 
-static void wfx_update_filtering_work(struct work_struct *work)
+static void wfx_filter_mcast(struct wfx_vif *wvif, bool filter_mcast)
 {
-	struct wfx_vif *wvif = container_of(work, struct wfx_vif,
-					    update_filtering_work);
+	int i;
 
-	wfx_update_filtering(wvif);
+	// Temporary workaround for filters
+	hif_set_data_filtering(wvif, false, true);
+	return;
+
+	if (!filter_mcast) {
+		hif_set_data_filtering(wvif, false, true);
+		return;
+	}
+	for (i = 0; i < wvif->filter_mcast_count; i++)
+		hif_set_mac_addr_condition(wvif, i, wvif->filter_mcast_addr[i]);
+	hif_set_uc_mc_bc_condition(wvif, 0,
+				   HIF_FILTER_UNICAST | HIF_FILTER_BROADCAST);
+	hif_set_config_data_filter(wvif, true, 0, BIT(1),
+				   BIT(wvif->filter_mcast_count) - 1);
+	hif_set_data_filtering(wvif, true, true);
 }
 
 u64 wfx_prepare_multicast(struct ieee80211_hw *hw,
@@ -213,72 +122,127 @@ u64 wfx_prepare_multicast(struct ieee80211_hw *hw,
 	int count = netdev_hw_addr_list_count(mc_list);
 
 	while ((wvif = wvif_iterate(wdev, wvif)) != NULL) {
-		memset(&wvif->mcast_filter, 0x00, sizeof(wvif->mcast_filter));
-		if (!count ||
-		    count > ARRAY_SIZE(wvif->mcast_filter.address_list))
+		if (count > ARRAY_SIZE(wvif->filter_mcast_addr)) {
+			wvif->filter_mcast_count = 0;
 			continue;
+		}
+		wvif->filter_mcast_count = count;
 
 		i = 0;
 		netdev_hw_addr_list_for_each(ha, mc_list) {
-			ether_addr_copy(wvif->mcast_filter.address_list[i],
-					ha->addr);
+			ether_addr_copy(wvif->filter_mcast_addr[i], ha->addr);
 			i++;
 		}
-		wvif->mcast_filter.enable = true;
-		wvif->mcast_filter.num_addresses = count;
 	}
 
 	return 0;
 }
 
-void wfx_configure_filter(struct ieee80211_hw *hw,
-			     unsigned int changed_flags,
-			     unsigned int *total_flags,
-			     u64 unused)
+void wfx_configure_filter(struct ieee80211_hw *hw, unsigned int changed_flags,
+			  unsigned int *total_flags, u64 unused)
 {
 	struct wfx_vif *wvif = NULL;
 	struct wfx_dev *wdev = hw->priv;
+	bool filter_bssid, filter_prbreq, filter_beacon, filter_mcast;
+
+	// Notes:
+	//   - Probe responses (FIF_BCN_PRBRESP_PROMISC) are never filtered
+	//   - PS-Poll (FIF_PSPOLL) are never filtered
+	//   - RTS, CTS and Ack (FIF_CONTROL) are always filtered
+	//   - Broken frames (FIF_FCSFAIL and FIF_PLCPFAIL) are always filtered
+	//   - Firmware does (yet) allow to forward unicast traffic sent to
+	//     other stations (aka. promiscuous mode)
+	*total_flags &= FIF_BCN_PRBRESP_PROMISC | FIF_ALLMULTI | FIF_OTHER_BSS |
+			FIF_PROBE_REQ | FIF_PSPOLL;
 
-	*total_flags &= FIF_OTHER_BSS | FIF_FCSFAIL | FIF_PROBE_REQ;
-
+	mutex_lock(&wdev->conf_mutex);
 	while ((wvif = wvif_iterate(wdev, wvif)) != NULL) {
 		mutex_lock(&wvif->scan_lock);
-		wvif->filter_bssid = (*total_flags &
-				      (FIF_OTHER_BSS | FIF_PROBE_REQ)) ? 0 : 1;
-		wvif->disable_beacon_filter = !(*total_flags & FIF_PROBE_REQ);
-		wfx_fwd_probe_req(wvif, true);
-		wfx_update_filtering(wvif);
+
+		// Note: FIF_BCN_PRBRESP_PROMISC covers probe response and
+		// beacons from other BSS
+		if (*total_flags & FIF_BCN_PRBRESP_PROMISC)
+			filter_beacon = false;
+		else
+			filter_beacon = true;
+		wfx_filter_beacon(wvif, filter_beacon);
+
+		if (*total_flags & FIF_ALLMULTI) {
+			filter_mcast = false;
+		} else if (!wvif->filter_mcast_count) {
+			dev_dbg(wdev->dev, "disabling unconfigured multicast filter");
+			filter_mcast = false;
+		} else {
+			filter_mcast = true;
+		}
+		wfx_filter_mcast(wvif, filter_mcast);
+
+		if (*total_flags & FIF_OTHER_BSS)
+			filter_bssid = false;
+		else
+			filter_bssid = true;
+
+		// In AP mode, chip can reply to probe request itself
+		if (*total_flags & FIF_PROBE_REQ &&
+		    wvif->vif->type == NL80211_IFTYPE_AP) {
+			dev_dbg(wdev->dev, "do not forward probe request in AP mode\n");
+			*total_flags &= ~FIF_PROBE_REQ;
+		}
+
+		if (*total_flags & FIF_PROBE_REQ)
+			filter_prbreq = false;
+		else
+			filter_prbreq = true;
+		hif_set_rx_filter(wvif, filter_bssid, filter_prbreq);
+
 		mutex_unlock(&wvif->scan_lock);
 	}
+	mutex_unlock(&wdev->conf_mutex);
 }
 
-static int wfx_update_pm(struct wfx_vif *wvif)
+int wfx_get_ps_timeout(struct wfx_vif *wvif, bool *enable_ps)
 {
-	struct ieee80211_conf *conf = &wvif->wdev->hw->conf;
-	bool ps = conf->flags & IEEE80211_CONF_PS;
-	int ps_timeout = conf->dynamic_ps_timeout;
 	struct ieee80211_channel *chan0 = NULL, *chan1 = NULL;
+	struct ieee80211_conf *conf = &wvif->wdev->hw->conf;
 
-	WARN_ON(conf->dynamic_ps_timeout < 0);
-	if (wvif->state != WFX_STATE_STA || !wvif->bss_params.aid)
-		return 0;
-	if (!ps)
-		ps_timeout = 0;
-	if (wvif->uapsd_mask)
-		ps_timeout = 0;
-
-	// Kernel disable powersave when an AP is in use. In contrary, it is
-	// absolutely necessary to enable legacy powersave for WF200 if channels
-	// are differents.
+	WARN(!wvif->vif->bss_conf.assoc && enable_ps,
+	     "enable_ps is reliable only if associated");
 	if (wdev_to_wvif(wvif->wdev, 0))
 		chan0 = wdev_to_wvif(wvif->wdev, 0)->vif->bss_conf.chandef.chan;
 	if (wdev_to_wvif(wvif->wdev, 1))
 		chan1 = wdev_to_wvif(wvif->wdev, 1)->vif->bss_conf.chandef.chan;
 	if (chan0 && chan1 && chan0->hw_value != chan1->hw_value &&
 	    wvif->vif->type != NL80211_IFTYPE_AP) {
-		ps = true;
-		ps_timeout = 0;
+		// It is necessary to enable powersave if channels
+		// are differents.
+		if (enable_ps)
+			*enable_ps = true;
+		if (wvif->bss_not_support_ps_poll)
+			return 30;
+		else
+			return 0;
 	}
+	if (enable_ps)
+		*enable_ps = wvif->vif->bss_conf.ps;
+	if (wvif->vif->bss_conf.assoc && wvif->vif->bss_conf.ps)
+		return conf->dynamic_ps_timeout;
+	else
+		return -1;
+}
+
+int wfx_update_pm(struct wfx_vif *wvif)
+{
+	int ps_timeout;
+	bool ps;
+
+	if (!wvif->vif->bss_conf.assoc)
+		return 0;
+	ps_timeout = wfx_get_ps_timeout(wvif, &ps);
+	if (!ps)
+		ps_timeout = 0;
+	WARN_ON(ps_timeout < 0);
+	if (wvif->uapsd_mask)
+		ps_timeout = 0;
 
 	if (!wait_for_completion_timeout(&wvif->set_pm_mode_complete,
 					 TU_TO_JIFFIES(512)))
@@ -287,18 +251,25 @@ static 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)
 {
 	struct wfx_dev *wdev = hw->priv;
-	struct wfx_vif *wvif = (struct wfx_vif *) vif->drv_priv;
+	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
 	int old_uapsd = wvif->uapsd_mask;
 
 	WARN_ON(queue >= hw->queues);
 
 	mutex_lock(&wdev->conf_mutex);
 	assign_bit(queue, &wvif->uapsd_mask, params->uapsd);
-	memcpy(&wvif->edca_params[queue], params, sizeof(*params));
 	hif_set_edca_queue_params(wvif, queue, params);
 	if (wvif->vif->type == NL80211_IFTYPE_STATION &&
 	    old_uapsd != wvif->uapsd_mask) {
@@ -319,32 +290,9 @@ int wfx_set_rts_threshold(struct ieee80211_hw *hw, u32 value)
 	return 0;
 }
 
-static int __wfx_flush(struct wfx_dev *wdev, bool drop)
-{
-	for (;;) {
-		if (drop)
-			wfx_tx_queues_clear(wdev);
-		if (wait_event_timeout(wdev->tx_queue_stats.wait_link_id_empty,
-				       wfx_tx_queues_is_empty(wdev),
-				       2 * HZ) <= 0)
-			return -ETIMEDOUT;
-		wfx_tx_flush(wdev);
-		if (wfx_tx_queues_is_empty(wdev))
-			return 0;
-		dev_warn(wdev->dev, "frames queued while flushing tx queues");
-	}
-}
-
-void wfx_flush(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
-		  u32 queues, bool drop)
-{
-	// FIXME: only flush requested vif and queues
-	__wfx_flush(hw->priv, drop);
-}
-
 /* WSM callbacks */
 
-static void wfx_event_report_rssi(struct wfx_vif *wvif, u8 raw_rcpi_rssi)
+void wfx_event_report_rssi(struct wfx_vif *wvif, u8 raw_rcpi_rssi)
 {
 	/* RSSI: signed Q8.0, RCPI: unsigned Q7.1
 	 * RSSI = RCPI / 2 - 110
@@ -360,100 +308,23 @@ static void wfx_event_report_rssi(struct wfx_vif *wvif, u8 raw_rcpi_rssi)
 	ieee80211_cqm_rssi_notify(wvif->vif, cqm_evt, rcpi_rssi, GFP_KERNEL);
 }
 
-static void wfx_event_handler_work(struct work_struct *work)
+static void wfx_beacon_loss_work(struct work_struct *work)
 {
-	struct wfx_vif *wvif =
-		container_of(work, struct wfx_vif, event_handler_work);
-	struct wfx_hif_event *event;
-
-	LIST_HEAD(list);
-
-	spin_lock(&wvif->event_queue_lock);
-	list_splice_init(&wvif->event_queue, &list);
-	spin_unlock(&wvif->event_queue_lock);
-
-	list_for_each_entry(event, &list, link) {
-		switch (event->evt.event_id) {
-		case HIF_EVENT_IND_BSSLOST:
-			cancel_work_sync(&wvif->unjoin_work);
-			mutex_lock(&wvif->scan_lock);
-			wfx_cqm_bssloss_sm(wvif, 1, 0, 0);
-			mutex_unlock(&wvif->scan_lock);
-			break;
-		case HIF_EVENT_IND_BSSREGAINED:
-			wfx_cqm_bssloss_sm(wvif, 0, 0, 0);
-			cancel_work_sync(&wvif->unjoin_work);
-			break;
-		case HIF_EVENT_IND_RCPI_RSSI:
-			wfx_event_report_rssi(wvif,
-					      event->evt.event_data.rcpi_rssi);
-			break;
-		case HIF_EVENT_IND_PS_MODE_ERROR:
-			dev_warn(wvif->wdev->dev,
-				 "error while processing power save request\n");
-			break;
-		default:
-			dev_warn(wvif->wdev->dev,
-				 "unhandled event indication: %.2x\n",
-				 event->evt.event_id);
-			break;
-		}
-	}
-	__wfx_free_event_queue(&list);
-}
-
-static void wfx_bss_loss_work(struct work_struct *work)
-{
-	struct wfx_vif *wvif = container_of(work, struct wfx_vif,
-					    bss_loss_work.work);
-
-	ieee80211_connection_loss(wvif->vif);
-}
-
-static void wfx_bss_params_work(struct work_struct *work)
-{
-	struct wfx_vif *wvif = container_of(work, struct wfx_vif,
-					    bss_params_work);
+	struct wfx_vif *wvif = container_of(to_delayed_work(work),
+					    struct wfx_vif, beacon_loss_work);
+	struct ieee80211_bss_conf *bss_conf = &wvif->vif->bss_conf;
 
-	mutex_lock(&wvif->wdev->conf_mutex);
-	wvif->bss_params.bss_flags.lost_count_only = 1;
-	hif_set_bss_params(wvif, &wvif->bss_params);
-	wvif->bss_params.bss_flags.lost_count_only = 0;
-	mutex_unlock(&wvif->wdev->conf_mutex);
+	ieee80211_beacon_loss(wvif->vif);
+	schedule_delayed_work(to_delayed_work(work),
+			      msecs_to_jiffies(bss_conf->beacon_int));
 }
 
-static void wfx_do_unjoin(struct wfx_vif *wvif)
+void wfx_set_default_unicast_key(struct ieee80211_hw *hw,
+				 struct ieee80211_vif *vif, int idx)
 {
-	mutex_lock(&wvif->wdev->conf_mutex);
-
-	if (!wvif->state)
-		goto done;
-
-	if (wvif->state == WFX_STATE_AP)
-		goto done;
-
-	cancel_work_sync(&wvif->update_filtering_work);
-	wvif->state = WFX_STATE_PASSIVE;
-
-	/* Unjoin is a reset. */
-	wfx_tx_flush(wvif->wdev);
-	hif_keep_alive_period(wvif, 0);
-	hif_reset(wvif, false);
-	wfx_tx_policy_init(wvif);
-	hif_set_macaddr(wvif, wvif->vif->addr);
-	wfx_free_event_queue(wvif);
-	cancel_work_sync(&wvif->event_handler_work);
-	wfx_cqm_bssloss_sm(wvif, 0, 0, 0);
+	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
 
-	/* Disable Block ACKs */
-	hif_set_block_ack_policy(wvif, 0, 0);
-
-	wvif->disable_beacon_filter = false;
-	wfx_update_filtering(wvif);
-	memset(&wvif->bss_params, 0, sizeof(wvif->bss_params));
-
-done:
-	mutex_unlock(&wvif->wdev->conf_mutex);
+	hif_wep_default_key_id(wvif, idx);
 }
 
 static void wfx_set_mfp(struct wfx_vif *wvif,
@@ -472,8 +343,7 @@ static void wfx_set_mfp(struct wfx_vif *wvif,
 
 	rcu_read_lock();
 	if (bss)
-		ptr = (const u16 *) ieee80211_bss_get_ie(bss,
-							      WLAN_EID_RSN);
+		ptr = (const u16 *)ieee80211_bss_get_ie(bss, WLAN_EID_RSN);
 
 	if (ptr) {
 		ptr += pairwise_cipher_suite_count_offset;
@@ -487,6 +357,24 @@ static void wfx_set_mfp(struct wfx_vif *wvif,
 	hif_set_mfp(wvif, mfpc, mfpr);
 }
 
+void wfx_reset(struct wfx_vif *wvif)
+{
+	struct wfx_dev *wdev = wvif->wdev;
+
+	wfx_tx_lock_flush(wdev);
+	hif_reset(wvif, false);
+	wfx_tx_policy_init(wvif);
+	if (wvif_count(wdev) <= 1)
+		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)
+		wfx_update_pm(wvif);
+}
+
 static void wfx_do_join(struct wfx_vif *wvif)
 {
 	int ret;
@@ -498,9 +386,6 @@ static void wfx_do_join(struct wfx_vif *wvif)
 
 	wfx_tx_lock_flush(wvif->wdev);
 
-	if (wvif->state)
-		wfx_do_unjoin(wvif);
-
 	bss = cfg80211_get_bss(wvif->wdev->hw->wiphy, wvif->channel,
 			       conf->bssid, NULL, 0,
 			       IEEE80211_BSS_TYPE_ANY, IEEE80211_PRIVACY_ANY);
@@ -509,109 +394,72 @@ static void wfx_do_join(struct wfx_vif *wvif)
 		return;
 	}
 
-	mutex_lock(&wvif->wdev->conf_mutex);
-
-	/* Sanity check beacon interval */
-	if (!wvif->beacon_int)
-		wvif->beacon_int = 1;
-
 	rcu_read_lock(); // protect ssidie
-	if (!conf->ibss_joined)
+	if (bss)
 		ssidie = ieee80211_bss_get_ie(bss, WLAN_EID_SSID);
 	if (ssidie) {
 		ssidlen = ssidie[1];
-		memcpy(ssid, &ssidie[2], ssidie[1]);
+		if (ssidlen > IEEE80211_MAX_SSID_LEN)
+			ssidlen = IEEE80211_MAX_SSID_LEN;
+		memcpy(ssid, &ssidie[2], ssidlen);
 	}
 	rcu_read_unlock();
 
-	wfx_tx_flush(wvif->wdev);
-
-	if (wvif_count(wvif->wdev) <= 1)
-		hif_set_block_ack_policy(wvif, 0xFF, 0xFF);
-
 	wfx_set_mfp(wvif, bss);
+	cfg80211_put_bss(wvif->wdev->hw->wiphy, bss);
 
-	wvif->wdev->tx_burst_idx = -1;
+	wvif->join_in_progress = true;
 	ret = hif_join(wvif, conf, wvif->channel, ssid, ssidlen);
 	if (ret) {
 		ieee80211_connection_loss(wvif->vif);
-		wvif->join_complete_status = -1;
-		/* Tx lock still held, unjoin will clear it. */
-		if (!schedule_work(&wvif->unjoin_work))
-			wfx_tx_unlock(wvif->wdev);
+		wfx_reset(wvif);
 	} else {
-		wvif->join_complete_status = 0;
-		if (wvif->vif->type == NL80211_IFTYPE_ADHOC)
-			wvif->state = WFX_STATE_IBSS;
-		else
-			wvif->state = WFX_STATE_PRE_STA;
-		wfx_tx_unlock(wvif->wdev);
-
-		/* Upload keys */
-		wfx_upload_keys(wvif);
-
 		/* Due to beacon filtering it is possible that the
 		 * AP's beacon is not known for the mac80211 stack.
 		 * Disable filtering temporary to make sure the stack
 		 * receives at least one
 		 */
-		wvif->disable_beacon_filter = true;
+		wfx_filter_beacon(wvif, false);
 	}
-	wfx_update_filtering(wvif);
-
-	mutex_unlock(&wvif->wdev->conf_mutex);
-	if (bss)
-		cfg80211_put_bss(wvif->wdev->hw->wiphy, bss);
-}
-
-static void wfx_unjoin_work(struct work_struct *work)
-{
-	struct wfx_vif *wvif = container_of(work, struct wfx_vif, unjoin_work);
-
-	wfx_do_unjoin(wvif);
 	wfx_tx_unlock(wvif->wdev);
 }
 
 int wfx_sta_add(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 		struct ieee80211_sta *sta)
 {
-	struct wfx_vif *wvif = (struct wfx_vif *) vif->drv_priv;
-	struct wfx_sta_priv *sta_priv = (struct wfx_sta_priv *) &sta->drv_priv;
+	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;
 
-	// FIXME: in station mode, the current API interprets new link-id as a
-	// tdls peer.
-	if (vif->type == NL80211_IFTYPE_STATION)
+	// In station mode, the firmware interprets new link-id as a TDLS peer.
+	if (vif->type == NL80211_IFTYPE_STATION && !sta->tdls)
 		return 0;
 	sta_priv->link_id = ffz(wvif->link_id_map);
 	wvif->link_id_map |= BIT(sta_priv->link_id);
 	WARN_ON(!sta_priv->link_id);
-	WARN_ON(sta_priv->link_id >= WFX_MAX_STA_IN_AP_MODE);
+	WARN_ON(sta_priv->link_id >= HIF_LINK_ID_MAX);
 	hif_map_link(wvif, sta->addr, 0, sta_priv->link_id);
 
-	spin_lock_bh(&wvif->ps_state_lock);
-	if ((sta->uapsd_queues & IEEE80211_WMM_IE_STA_QOSINFO_AC_MASK) ==
-					IEEE80211_WMM_IE_STA_QOSINFO_AC_MASK)
-		wvif->sta_asleep_mask |= BIT(sta_priv->link_id);
-	spin_unlock_bh(&wvif->ps_state_lock);
 	return 0;
 }
 
 int wfx_sta_remove(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 		   struct ieee80211_sta *sta)
 {
-	struct wfx_vif *wvif = (struct wfx_vif *) vif->drv_priv;
-	struct wfx_sta_priv *sta_priv = (struct wfx_sta_priv *) &sta->drv_priv;
+	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])
-			dev_warn(wvif->wdev->dev, "release station while %d pending frame on queue %d",
-				 sta_priv->buffered[i], i);
-	// FIXME: see note in wfx_sta_add()
-	if (vif->type == NL80211_IFTYPE_STATION)
+			// 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;
 	// FIXME add a mutex?
 	hif_map_link(wvif, sta->addr, 1, sta_priv->link_id);
@@ -619,50 +467,10 @@ int wfx_sta_remove(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	return 0;
 }
 
-static int wfx_start_ap(struct wfx_vif *wvif)
-{
-	int ret;
-
-	wvif->beacon_int = wvif->vif->bss_conf.beacon_int;
-	wvif->wdev->tx_burst_idx = -1;
-	ret = hif_start(wvif, &wvif->vif->bss_conf, wvif->channel);
-	if (ret)
-		return ret;
-	ret = wfx_upload_keys(wvif);
-	if (ret)
-		return ret;
-	if (wvif_count(wvif->wdev) <= 1)
-		hif_set_block_ack_policy(wvif, 0xFF, 0xFF);
-	wvif->state = WFX_STATE_AP;
-	wfx_update_filtering(wvif);
-	return 0;
-}
-
-static int wfx_update_beaconing(struct wfx_vif *wvif)
-{
-	if (wvif->vif->type != NL80211_IFTYPE_AP)
-		return 0;
-	if (wvif->state == WFX_STATE_AP &&
-	    wvif->beacon_int == wvif->vif->bss_conf.beacon_int)
-		return 0;
-	wfx_tx_lock_flush(wvif->wdev);
-	hif_reset(wvif, false);
-	wfx_tx_policy_init(wvif);
-	wvif->state = WFX_STATE_PASSIVE;
-	wfx_start_ap(wvif);
-	wfx_tx_unlock(wvif->wdev);
-	return 0;
-}
-
 static int wfx_upload_ap_templates(struct wfx_vif *wvif)
 {
 	struct sk_buff *skb;
 
-	if (wvif->vif->type == NL80211_IFTYPE_STATION ||
-	    wvif->vif->type == NL80211_IFTYPE_MONITOR ||
-	    wvif->vif->type == NL80211_IFTYPE_UNSPECIFIED)
-		return 0;
-
 	skb = ieee80211_beacon_get(wvif->wdev->hw, wvif->vif);
 	if (!skb)
 		return -ENOMEM;
@@ -679,52 +487,77 @@ static int wfx_upload_ap_templates(struct wfx_vif *wvif)
 	return 0;
 }
 
+int wfx_start_ap(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+{
+	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
+	struct wfx_dev *wdev = wvif->wdev;
+	int ret;
+
+	wvif =  NULL;
+	while ((wvif = wvif_iterate(wdev, wvif)) != NULL)
+		wfx_update_pm(wvif);
+	wvif = (struct wfx_vif *)vif->drv_priv;
+	wfx_upload_ap_templates(wvif);
+	ret = hif_start(wvif, &vif->bss_conf, wvif->channel);
+	if (ret > 0)
+		return -EIO;
+	return ret;
+}
+
+void wfx_stop_ap(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+{
+	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
+
+	wfx_reset(wvif);
+}
+
 static void wfx_join_finalize(struct wfx_vif *wvif,
 			      struct ieee80211_bss_conf *info)
 {
-	struct ieee80211_sta *sta = NULL;
-
-	wvif->beacon_int = info->beacon_int;
-	rcu_read_lock(); // protect sta
-	if (info->bssid && !info->ibss_joined)
-		sta = ieee80211_find_sta(wvif->vif, info->bssid);
-	if (sta)
-		wvif->bss_params.operational_rate_set =
-			wfx_rate_mask_to_hw(wvif->wdev, sta->supp_rates[wvif->channel->band]);
-	else
-		wvif->bss_params.operational_rate_set = -1;
-	rcu_read_unlock();
-	if (sta &&
-	    info->ht_operation_mode & IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT)
-		hif_dual_cts_protection(wvif, true);
-	else
-		hif_dual_cts_protection(wvif, false);
+	wvif->join_in_progress = false;
+	hif_set_association_mode(wvif, info);
+	hif_keep_alive_period(wvif, 0);
+	// beacon_loss_count is defined to 7 in net/mac80211/mlme.c. Let's use
+	// the same value.
+	hif_set_bss_params(wvif, info->aid, 7);
+	hif_set_beacon_wakeup_period(wvif, 1, 1);
+	wfx_update_pm(wvif);
+}
 
-	wfx_cqm_bssloss_sm(wvif, 0, 0, 0);
-	cancel_work_sync(&wvif->unjoin_work);
+int wfx_join_ibss(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+{
+	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
 
-	wvif->bss_params.beacon_lost_count = 20;
-	wvif->bss_params.aid = info->aid;
+	wfx_upload_ap_templates(wvif);
+	wfx_do_join(wvif);
+	return 0;
+}
 
-	hif_set_association_mode(wvif, info);
+void wfx_leave_ibss(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+{
+	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
 
-	if (!info->ibss_joined) {
-		hif_keep_alive_period(wvif, 30 /* sec */);
-		hif_set_bss_params(wvif, &wvif->bss_params);
-		hif_set_beacon_wakeup_period(wvif, info->dtim_period,
-					     info->dtim_period);
-		wfx_update_pm(wvif);
+	wfx_reset(wvif);
+}
+
+static void wfx_enable_beacon(struct wfx_vif *wvif, bool enable)
+{
+	// Driver has Content After DTIM Beacon in queue. Driver is waiting for
+	// a signal from the firmware. Since we are going to stop to send
+	// beacons, this signal will never happens. See also
+	// wfx_suspend_resume_mc()
+	if (!enable && wfx_tx_queues_has_cab(wvif)) {
+		wvif->after_dtim_tx_allowed = true;
+		wfx_bh_request_tx(wvif->wdev);
 	}
+	hif_beacon_transmit(wvif, enable);
 }
 
-void wfx_bss_info_changed(struct ieee80211_hw *hw,
-			     struct ieee80211_vif *vif,
-			     struct ieee80211_bss_conf *info,
-			     u32 changed)
+void wfx_bss_info_changed(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+			  struct ieee80211_bss_conf *info, u32 changed)
 {
 	struct wfx_dev *wdev = hw->priv;
-	struct wfx_vif *wvif = (struct wfx_vif *) vif->drv_priv;
-	bool do_join = false;
+	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
 	int i;
 
 	mutex_lock(&wdev->conf_mutex);
@@ -742,92 +575,52 @@ void wfx_bss_info_changed(struct ieee80211_hw *hw,
 		}
 	}
 
-	if (changed & BSS_CHANGED_BEACON ||
-	    changed & BSS_CHANGED_AP_PROBE_RESP ||
-	    changed & BSS_CHANGED_BSSID ||
-	    changed & BSS_CHANGED_SSID ||
-	    changed & BSS_CHANGED_IBSS) {
-		wvif->beacon_int = info->beacon_int;
-		wfx_update_beaconing(wvif);
-		wfx_upload_ap_templates(wvif);
-		wfx_fwd_probe_req(wvif, false);
+	if (changed & BSS_CHANGED_BASIC_RATES ||
+	    changed & BSS_CHANGED_BEACON_INT ||
+	    changed & BSS_CHANGED_BSSID) {
+		if (vif->type == NL80211_IFTYPE_STATION)
+			wfx_do_join(wvif);
 	}
 
-	if (changed & BSS_CHANGED_BEACON_ENABLED &&
-	    wvif->state != WFX_STATE_IBSS)
-		hif_beacon_transmit(wvif, info->enable_beacon);
+	if (changed & BSS_CHANGED_AP_PROBE_RESP ||
+	    changed & BSS_CHANGED_BEACON)
+		wfx_upload_ap_templates(wvif);
+
+	if (changed & BSS_CHANGED_BEACON_ENABLED)
+		wfx_enable_beacon(wvif, info->enable_beacon);
 
-	if (changed & BSS_CHANGED_BEACON_INFO)
+	if (changed & BSS_CHANGED_BEACON_INFO) {
+		if (vif->type != NL80211_IFTYPE_STATION)
+			dev_warn(wdev->dev, "%s: misunderstood change: BEACON_INFO\n",
+				 __func__);
 		hif_set_beacon_wakeup_period(wvif, info->dtim_period,
 					     info->dtim_period);
-
-	/* assoc/disassoc, or maybe AID changed */
-	if (changed & BSS_CHANGED_ASSOC) {
-		wfx_tx_lock_flush(wdev);
-		wvif->wep_default_key_id = -1;
-		wfx_tx_unlock(wdev);
+		// We temporary forwarded beacon for join process. It is now no
+		// more necessary.
+		wfx_filter_beacon(wvif, true);
 	}
 
-	if (changed & BSS_CHANGED_ASSOC && !info->assoc &&
-	    (wvif->state == WFX_STATE_STA || wvif->state == WFX_STATE_IBSS)) {
-		/* Shedule unjoin work */
-		wfx_tx_lock(wdev);
-		if (!schedule_work(&wvif->unjoin_work))
-			wfx_tx_unlock(wdev);
-	} else {
-		if (changed & BSS_CHANGED_BEACON_INT) {
-			if (info->ibss_joined)
-				do_join = true;
-			else if (wvif->state == WFX_STATE_AP)
-				wfx_update_beaconing(wvif);
-		}
-
-		if (changed & BSS_CHANGED_BSSID)
-			do_join = true;
-
-		if (changed & BSS_CHANGED_ASSOC ||
-		    changed & BSS_CHANGED_BSSID ||
-		    changed & BSS_CHANGED_IBSS ||
-		    changed & BSS_CHANGED_BASIC_RATES ||
-		    changed & BSS_CHANGED_HT) {
-			if (info->assoc) {
-				if (wvif->state < WFX_STATE_PRE_STA) {
-					ieee80211_connection_loss(vif);
-					mutex_unlock(&wdev->conf_mutex);
-					return;
-				} else if (wvif->state == WFX_STATE_PRE_STA) {
-					wvif->state = WFX_STATE_STA;
-				}
-			} else {
-				do_join = true;
-			}
-
-			if (info->assoc || info->ibss_joined)
-				wfx_join_finalize(wvif, info);
-			else
-				memset(&wvif->bss_params, 0,
-				       sizeof(wvif->bss_params));
-		}
+	if (changed & BSS_CHANGED_ASSOC) {
+		if (info->assoc || info->ibss_joined)
+			wfx_join_finalize(wvif, info);
+		else if (!info->assoc && vif->type == NL80211_IFTYPE_STATION)
+			wfx_reset(wvif);
+		else
+			dev_warn(wdev->dev, "%s: misunderstood change: ASSOC\n",
+				 __func__);
 	}
 
-	if (changed & BSS_CHANGED_ASSOC ||
-	    changed & BSS_CHANGED_ERP_CTS_PROT ||
-	    changed & BSS_CHANGED_ERP_PREAMBLE) {
-		u8 erp_ie[3] = { WLAN_EID_ERP_INFO, 1, 0 };
+	if (changed & BSS_CHANGED_KEEP_ALIVE)
+		hif_keep_alive_period(wvif, info->max_idle_period *
+					    USEC_PER_TU / USEC_PER_MSEC);
 
+	if (changed & BSS_CHANGED_ERP_CTS_PROT)
 		hif_erp_use_protection(wvif, info->use_cts_prot);
-		if (info->use_cts_prot)
-			erp_ie[2] |= WLAN_ERP_USE_PROTECTION;
-		if (info->use_short_preamble)
-			erp_ie[2] |= WLAN_ERP_BARKER_PREAMBLE;
-		if (wvif->vif->type != NL80211_IFTYPE_STATION)
-			hif_update_ie_beacon(wvif, erp_ie, sizeof(erp_ie));
-	}
 
-	if (changed & BSS_CHANGED_ASSOC || changed & BSS_CHANGED_ERP_SLOT)
+	if (changed & BSS_CHANGED_ERP_SLOT)
 		hif_slot_time(wvif, info->use_short_slot ? 9 : 20);
 
-	if (changed & BSS_CHANGED_ASSOC || changed & BSS_CHANGED_CQM)
+	if (changed & BSS_CHANGED_CQM)
 		hif_set_rcpi_rssi_threshold(wvif, info->cqm_rssi_thold,
 					    info->cqm_rssi_hyst);
 
@@ -838,31 +631,6 @@ void wfx_bss_info_changed(struct ieee80211_hw *hw,
 		wfx_update_pm(wvif);
 
 	mutex_unlock(&wdev->conf_mutex);
-
-	if (do_join)
-		wfx_do_join(wvif);
-}
-
-static void wfx_ps_notify_sta(struct wfx_vif *wvif,
-			      enum sta_notify_cmd notify_cmd, int link_id)
-{
-	spin_lock_bh(&wvif->ps_state_lock);
-	if (notify_cmd == STA_NOTIFY_SLEEP)
-		wvif->sta_asleep_mask |= BIT(link_id);
-	else // notify_cmd == STA_NOTIFY_AWAKE
-		wvif->sta_asleep_mask &= ~BIT(link_id);
-	spin_unlock_bh(&wvif->ps_state_lock);
-	if (notify_cmd == STA_NOTIFY_AWAKE)
-		wfx_bh_request_tx(wvif->wdev);
-}
-
-void wfx_sta_notify(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
-		    enum sta_notify_cmd notify_cmd, struct ieee80211_sta *sta)
-{
-	struct wfx_vif *wvif = (struct wfx_vif *) vif->drv_priv;
-	struct wfx_sta_priv *sta_priv = (struct wfx_sta_priv *) &sta->drv_priv;
-
-	wfx_ps_notify_sta(wvif, notify_cmd, sta_priv->link_id);
 }
 
 static int wfx_update_tim(struct wfx_vif *wvif)
@@ -873,10 +641,8 @@ static int wfx_update_tim(struct wfx_vif *wvif)
 
 	skb = ieee80211_beacon_get_tim(wvif->wdev->hw, wvif->vif,
 				       &tim_offset, &tim_length);
-	if (!skb) {
-		__wfx_flush(wvif->wdev, true);
+	if (!skb)
 		return -ENOENT;
-	}
 	tim_ptr = skb->data + tim_offset;
 
 	if (tim_offset && tim_length >= 6) {
@@ -886,7 +652,7 @@ static int wfx_update_tim(struct wfx_vif *wvif)
 		tim_ptr[2] = 0;
 
 		/* Set/reset aid0 bit */
-		if (wfx_tx_queues_get_after_dtim(wvif))
+		if (wfx_tx_queues_has_cab(wvif))
 			tim_ptr[4] |= 1;
 		else
 			tim_ptr[4] &= ~1;
@@ -917,7 +683,9 @@ int wfx_set_tim(struct ieee80211_hw *hw, struct ieee80211_sta *sta, bool set)
 
 void wfx_suspend_resume_mc(struct wfx_vif *wvif, enum sta_notify_cmd notify_cmd)
 {
-	WARN(!wfx_tx_queues_get_after_dtim(wvif), "incorrect sequence");
+	if (notify_cmd != STA_NOTIFY_AWAKE)
+		return;
+	WARN(!wfx_tx_queues_has_cab(wvif), "incorrect sequence");
 	WARN(wvif->after_dtim_tx_allowed, "incorrect sequence");
 	wvif->after_dtim_tx_allowed = true;
 	wfx_bh_request_tx(wvif->wdev);
@@ -958,7 +726,7 @@ void wfx_change_chanctx(struct ieee80211_hw *hw,
 int wfx_assign_vif_chanctx(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 			   struct ieee80211_chanctx_conf *conf)
 {
-	struct wfx_vif *wvif = (struct wfx_vif *) vif->drv_priv;
+	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
 	struct ieee80211_channel *ch = conf->def.chan;
 
 	WARN(wvif->channel, "channel overwrite");
@@ -971,7 +739,7 @@ void wfx_unassign_vif_chanctx(struct ieee80211_hw *hw,
 			      struct ieee80211_vif *vif,
 			      struct ieee80211_chanctx_conf *conf)
 {
-	struct wfx_vif *wvif = (struct wfx_vif *) vif->drv_priv;
+	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
 	struct ieee80211_channel *ch = conf->def.chan;
 
 	WARN(wvif->channel != ch, "channel mismatch");
@@ -987,7 +755,7 @@ int wfx_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 {
 	int i, ret = 0;
 	struct wfx_dev *wdev = hw->priv;
-	struct wfx_vif *wvif = (struct wfx_vif *) vif->drv_priv;
+	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
 
 	vif->driver_flags |= IEEE80211_VIF_BEACON_FILTER |
 			     IEEE80211_VIF_SUPPORTS_UAPSD |
@@ -1021,33 +789,18 @@ int wfx_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 	wvif->wdev = wdev;
 
 	wvif->link_id_map = 1; // link-id 0 is reserved for multicast
-	spin_lock_init(&wvif->ps_state_lock);
 	INIT_WORK(&wvif->update_tim_work, wfx_update_tim_work);
-
-	memset(&wvif->bss_params, 0, sizeof(wvif->bss_params));
-
-	mutex_init(&wvif->bss_loss_lock);
-	INIT_DELAYED_WORK(&wvif->bss_loss_work, wfx_bss_loss_work);
-
-	wvif->wep_default_key_id = -1;
-	INIT_WORK(&wvif->wep_key_work, wfx_wep_key_work);
-
-	spin_lock_init(&wvif->event_queue_lock);
-	INIT_LIST_HEAD(&wvif->event_queue);
-	INIT_WORK(&wvif->event_handler_work, wfx_event_handler_work);
+	INIT_DELAYED_WORK(&wvif->beacon_loss_work, wfx_beacon_loss_work);
 
 	init_completion(&wvif->set_pm_mode_complete);
 	complete(&wvif->set_pm_mode_complete);
-	INIT_WORK(&wvif->update_filtering_work, wfx_update_filtering_work);
-	INIT_WORK(&wvif->bss_params_work, wfx_bss_params_work);
-	INIT_WORK(&wvif->unjoin_work, wfx_unjoin_work);
+	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);
 	init_completion(&wvif->scan_complete);
 	INIT_WORK(&wvif->scan_work, wfx_hw_scan_work);
 
-	INIT_WORK(&wvif->tx_policy_upload_work, wfx_tx_policy_upload_work);
 	mutex_unlock(&wdev->conf_mutex);
 
 	hif_set_macaddr(wvif, vif->addr);
@@ -1060,50 +813,25 @@ int wfx_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 			hif_set_block_ack_policy(wvif, 0xFF, 0xFF);
 		else
 			hif_set_block_ack_policy(wvif, 0x00, 0x00);
-		// Combo force powersave mode. We can re-enable it now
-		ret = wfx_update_pm(wvif);
 	}
 	return ret;
 }
 
-void wfx_remove_interface(struct ieee80211_hw *hw,
-			  struct ieee80211_vif *vif)
+void wfx_remove_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 {
 	struct wfx_dev *wdev = hw->priv;
-	struct wfx_vif *wvif = (struct wfx_vif *) vif->drv_priv;
+	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
 
 	wait_for_completion_timeout(&wvif->set_pm_mode_complete, msecs_to_jiffies(300));
 
 	mutex_lock(&wdev->conf_mutex);
 	WARN(wvif->link_id_map != 1, "corrupted state");
-	switch (wvif->state) {
-	case WFX_STATE_PRE_STA:
-	case WFX_STATE_STA:
-	case WFX_STATE_IBSS:
-		wfx_tx_lock_flush(wdev);
-		if (!schedule_work(&wvif->unjoin_work))
-			wfx_tx_unlock(wdev);
-		break;
-	case WFX_STATE_AP:
-		wvif->sta_asleep_mask = 0;
-		/* reset.link_id = 0; */
-		hif_reset(wvif, false);
-		break;
-	default:
-		break;
-	}
-
-	wvif->state = WFX_STATE_PASSIVE;
-	wfx_tx_queues_wait_empty_vif(wvif);
-	wfx_tx_unlock(wdev);
 
-	/* FIXME: In add to reset MAC address, try to reset interface */
+	hif_reset(wvif, false);
 	hif_set_macaddr(wvif, NULL);
+	wfx_tx_policy_init(wvif);
 
-	wfx_cqm_bssloss_sm(wvif, 0, 0, 0);
-	cancel_work_sync(&wvif->unjoin_work);
-	wfx_free_event_queue(wvif);
-
+	cancel_delayed_work_sync(&wvif->beacon_loss_work);
 	wdev->vif[wvif->id] = NULL;
 	wvif->vif = NULL;
 
@@ -1115,8 +843,6 @@ void wfx_remove_interface(struct ieee80211_hw *hw,
 			hif_set_block_ack_policy(wvif, 0xFF, 0xFF);
 		else
 			hif_set_block_ack_policy(wvif, 0x00, 0x00);
-		// Combo force powersave mode. We can re-enable it now
-		wfx_update_pm(wvif);
 	}
 }
 
@@ -1129,10 +855,5 @@ void wfx_stop(struct ieee80211_hw *hw)
 {
 	struct wfx_dev *wdev = hw->priv;
 
-	wfx_tx_lock_flush(wdev);
-	mutex_lock(&wdev->conf_mutex);
-	wfx_tx_queues_clear(wdev);
-	mutex_unlock(&wdev->conf_mutex);
-	wfx_tx_unlock(wdev);
-	WARN(atomic_read(&wdev->tx_lock), "tx_lock is locked");
+	wfx_tx_queues_check_empty(wdev);
 }