staging: wfx: implement the rest of mac80211 API

Finish to fill struct ieee80211_ops with necessary callbacks. Driver is
now ready to be registered to mac80211.

Signed-off-by: Jérôme Pouiller <jerome.pouiller@silabs.com>
Link: https://lore.kernel.org/r/20190919142527.31797-21-Jerome.Pouiller@silabs.com
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

11 files changed, 1907 insertions, 3 deletions

diff --git a/drivers/staging/wfx/data_rx.c b/drivers/staging/wfx/data_rx.c
index 3b3117b2edac..3a79089c8501 100644
--- a/drivers/staging/wfx/data_rx.c
+++ b/drivers/staging/wfx/data_rx.c
@@ -21,6 +21,8 @@ static int wfx_handle_pspoll(struct wfx_vif *wvif, struct sk_buff *skb)
 	u32 pspoll_mask = 0;
 	int i;
 
+	if (wvif->state != WFX_STATE_AP)
+		return 1;
 	if (!ether_addr_equal(wvif->vif->addr, pspoll->bssid))
 		return 1;
 
@@ -162,6 +164,30 @@ void wfx_rx_cb(struct wfx_vif *wvif, struct hif_ind_rx *arg, struct sk_buff *skb
 	    && arg->rx_flags.match_uc_addr
 	    && mgmt->u.action.category == WLAN_CATEGORY_BACK)
 		goto drop;
+	if (ieee80211_is_beacon(frame->frame_control)
+	    && !arg->status && wvif->vif
+	    && ether_addr_equal(ieee80211_get_SA(frame), wvif->vif->bss_conf.bssid)) {
+		const u8 *tim_ie;
+		u8 *ies = mgmt->u.beacon.variable;
+		size_t ies_len = skb->len - (ies - skb->data);
+
+		tim_ie = cfg80211_find_ie(WLAN_EID_TIM, ies, ies_len);
+		if (tim_ie) {
+			struct ieee80211_tim_ie *tim = (struct ieee80211_tim_ie *) &tim_ie[2];
+
+			if (wvif->dtim_period != tim->dtim_period) {
+				wvif->dtim_period = tim->dtim_period;
+				schedule_work(&wvif->set_beacon_wakeup_period_work);
+			}
+		}
+
+		/* Disable beacon filter once we're associated... */
+		if (wvif->disable_beacon_filter &&
+		    (wvif->vif->bss_conf.assoc || wvif->vif->bss_conf.ibss_joined)) {
+			wvif->disable_beacon_filter = false;
+			schedule_work(&wvif->update_filtering_work);
+		}
+	}
 
 	if (early_data) {
 		spin_lock_bh(&wvif->ps_state_lock);
diff --git a/drivers/staging/wfx/data_tx.c b/drivers/staging/wfx/data_tx.c
index 217d3c270706..7f2799fbdafe 100644
--- a/drivers/staging/wfx/data_tx.c
+++ b/drivers/staging/wfx/data_tx.c
@@ -10,6 +10,7 @@
 #include "data_tx.h"
 #include "wfx.h"
 #include "bh.h"
+#include "sta.h"
 #include "queue.h"
 #include "debug.h"
 #include "traces.h"
@@ -359,6 +360,9 @@ void wfx_link_id_gc_work(struct work_struct *work)
 	u32 mask;
 	int i;
 
+	if (wvif->state != WFX_STATE_AP)
+		return;
+
 	wfx_tx_lock_flush(wvif->wdev);
 	spin_lock_bh(&wvif->ps_state_lock);
 	for (i = 0; i < WFX_MAX_STA_IN_AP_MODE; ++i) {
@@ -729,14 +733,26 @@ void wfx_tx_confirm_cb(struct wfx_vif *wvif, struct hif_cnf_tx *arg)
 	memset(tx_info->pad, 0, sizeof(tx_info->pad));
 
 	if (!arg->status) {
+		if (wvif->bss_loss_state && arg->packet_id == wvif->bss_loss_confirm_id)
+			wfx_cqm_bssloss_sm(wvif, 0, 1, 0);
 		tx_info->status.tx_time = arg->media_delay - arg->tx_queue_delay;
 		if (tx_info->flags & IEEE80211_TX_CTL_NO_ACK)
 			tx_info->flags |= IEEE80211_TX_STAT_NOACK_TRANSMITTED;
 		else
 			tx_info->flags |= IEEE80211_TX_STAT_ACK;
 	} else if (arg->status == HIF_REQUEUE) {
+		/* "REQUEUE" means "implicit suspend" */
+		struct hif_ind_suspend_resume_tx suspend = {
+			.suspend_resume_flags.resume = 0,
+			.suspend_resume_flags.bc_mc_only = 1,
+		};
+
 		WARN(!arg->tx_result_flags.requeue, "incoherent status and result_flags");
+		wfx_suspend_resume(wvif, &suspend);
 		tx_info->flags |= IEEE80211_TX_STAT_TX_FILTERED;
+	} else {
+		if (wvif->bss_loss_state && arg->packet_id == wvif->bss_loss_confirm_id)
+			wfx_cqm_bssloss_sm(wvif, 0, 0, 1);
 	}
 	wfx_pending_remove(wvif->wdev, skb);
 }
diff --git a/drivers/staging/wfx/debug.c b/drivers/staging/wfx/debug.c
index 1e23bb5bde3e..3261b267c385 100644
--- a/drivers/staging/wfx/debug.c
+++ b/drivers/staging/wfx/debug.c
@@ -11,7 +11,9 @@
 
 #include "debug.h"
 #include "wfx.h"
+#include "sta.h"
 #include "main.h"
+#include "hif_tx.h"
 #include "hif_tx_mib.h"
 
 #define CREATE_TRACE_POINTS
diff --git a/drivers/staging/wfx/hif_rx.c b/drivers/staging/wfx/hif_rx.c
index d386fab0a90f..52db02d3aa41 100644
--- a/drivers/staging/wfx/hif_rx.c
+++ b/drivers/staging/wfx/hif_rx.c
@@ -12,6 +12,8 @@
 #include "hif_rx.h"
 #include "wfx.h"
 #include "scan.h"
+#include "bh.h"
+#include "sta.h"
 #include "data_rx.h"
 #include "secure_link.h"
 #include "hif_api_cmd.h"
@@ -144,6 +146,43 @@ static int hif_receive_indication(struct wfx_dev *wdev, struct hif_msg *hif, voi
 	return 0;
 }
 
+static int hif_event_indication(struct wfx_dev *wdev, struct hif_msg *hif, void *buf)
+{
+	struct wfx_vif *wvif = wdev_to_wvif(wdev, hif->interface);
+	struct hif_ind_event *body = buf;
+	struct wfx_hif_event *event;
+	int first;
+
+	WARN_ON(!wvif);
+	if (!wvif)
+		return 0;
+
+	event = kzalloc(sizeof(*event), GFP_KERNEL);
+	if (!event)
+		return -ENOMEM;
+
+	memcpy(&event->evt, body, sizeof(struct hif_ind_event));
+	spin_lock(&wvif->event_queue_lock);
+	first = list_empty(&wvif->event_queue);
+	list_add_tail(&event->link, &wvif->event_queue);
+	spin_unlock(&wvif->event_queue_lock);
+
+	if (first)
+		schedule_work(&wvif->event_handler_work);
+
+	return 0;
+}
+
+static int hif_pm_mode_complete_indication(struct wfx_dev *wdev, struct hif_msg *hif, void *buf)
+{
+	struct wfx_vif *wvif = wdev_to_wvif(wdev, hif->interface);
+
+	WARN_ON(!wvif);
+	complete(&wvif->set_pm_mode_complete);
+
+	return 0;
+}
+
 static int hif_scan_complete_indication(struct wfx_dev *wdev, struct hif_msg *hif, void *buf)
 {
 	struct wfx_vif *wvif = wdev_to_wvif(wdev, hif->interface);
@@ -165,6 +204,17 @@ static int hif_join_complete_indication(struct wfx_dev *wdev, struct hif_msg *hi
 	return 0;
 }
 
+static int hif_suspend_resume_indication(struct wfx_dev *wdev, struct hif_msg *hif, void *buf)
+{
+	struct wfx_vif *wvif = wdev_to_wvif(wdev, hif->interface);
+	struct hif_ind_suspend_resume_tx *body = buf;
+
+	WARN_ON(!wvif);
+	wfx_suspend_resume(wvif, body);
+
+	return 0;
+}
+
 static int hif_error_indication(struct wfx_dev *wdev, struct hif_msg *hif, void *buf)
 {
 	struct hif_ind_error *body = buf;
@@ -242,8 +292,11 @@ static const struct {
 	{ HIF_IND_ID_STARTUP,              hif_startup_indication },
 	{ HIF_IND_ID_WAKEUP,               hif_wakeup_indication },
 	{ HIF_IND_ID_JOIN_COMPLETE,        hif_join_complete_indication },
+	{ HIF_IND_ID_SET_PM_MODE_CMPL,     hif_pm_mode_complete_indication },
 	{ HIF_IND_ID_SCAN_CMPL,            hif_scan_complete_indication },
+	{ HIF_IND_ID_SUSPEND_RESUME_TX,    hif_suspend_resume_indication },
 	{ HIF_IND_ID_SL_EXCHANGE_PUB_KEYS, hif_keys_indication },
+	{ HIF_IND_ID_EVENT,                hif_event_indication },
 	{ HIF_IND_ID_GENERIC,              hif_generic_indication },
 	{ HIF_IND_ID_ERROR,                hif_error_indication },
 	{ HIF_IND_ID_EXCEPTION,            hif_exception_indication },
diff --git a/drivers/staging/wfx/hif_tx.c b/drivers/staging/wfx/hif_tx.c
index 157ab177b73f..2d40225a0fce 100644
--- a/drivers/staging/wfx/hif_tx.c
+++ b/drivers/staging/wfx/hif_tx.c
@@ -14,6 +14,7 @@
 #include "bh.h"
 #include "hwio.h"
 #include "debug.h"
+#include "sta.h"
 
 void wfx_init_hif_cmd(struct wfx_hif_cmd *hif_cmd)
 {
diff --git a/drivers/staging/wfx/main.c b/drivers/staging/wfx/main.c
index e7bba24aae0b..fe9a89703897 100644
--- a/drivers/staging/wfx/main.c
+++ b/drivers/staging/wfx/main.c
@@ -50,14 +50,112 @@ static char *slk_key;
 module_param(slk_key, charp, 0600);
 MODULE_PARM_DESC(slk_key, "secret key for secure link (expect 64 hexdecimal digits).");
 
+#define RATETAB_ENT(_rate, _rateid, _flags) { \
+	.bitrate  = (_rate),   \
+	.hw_value = (_rateid), \
+	.flags    = (_flags),  \
+}
+
+static struct ieee80211_rate wfx_rates[] = {
+	RATETAB_ENT(10,  0,  0),
+	RATETAB_ENT(20,  1,  IEEE80211_RATE_SHORT_PREAMBLE),
+	RATETAB_ENT(55,  2,  IEEE80211_RATE_SHORT_PREAMBLE),
+	RATETAB_ENT(110, 3,  IEEE80211_RATE_SHORT_PREAMBLE),
+	RATETAB_ENT(60,  6,  0),
+	RATETAB_ENT(90,  7,  0),
+	RATETAB_ENT(120, 8,  0),
+	RATETAB_ENT(180, 9,  0),
+	RATETAB_ENT(240, 10, 0),
+	RATETAB_ENT(360, 11, 0),
+	RATETAB_ENT(480, 12, 0),
+	RATETAB_ENT(540, 13, 0),
+};
+
+#define CHAN2G(_channel, _freq, _flags) { \
+	.band = NL80211_BAND_2GHZ, \
+	.center_freq = (_freq),    \
+	.hw_value = (_channel),    \
+	.flags = (_flags),         \
+	.max_antenna_gain = 0,     \
+	.max_power = 30,           \
+}
+
+static struct ieee80211_channel wfx_2ghz_chantable[] = {
+	CHAN2G(1,  2412, 0),
+	CHAN2G(2,  2417, 0),
+	CHAN2G(3,  2422, 0),
+	CHAN2G(4,  2427, 0),
+	CHAN2G(5,  2432, 0),
+	CHAN2G(6,  2437, 0),
+	CHAN2G(7,  2442, 0),
+	CHAN2G(8,  2447, 0),
+	CHAN2G(9,  2452, 0),
+	CHAN2G(10, 2457, 0),
+	CHAN2G(11, 2462, 0),
+	CHAN2G(12, 2467, 0),
+	CHAN2G(13, 2472, 0),
+	CHAN2G(14, 2484, 0),
+};
+
+static const struct ieee80211_supported_band wfx_band_2ghz = {
+	.channels = wfx_2ghz_chantable,
+	.n_channels = ARRAY_SIZE(wfx_2ghz_chantable),
+	.bitrates = wfx_rates,
+	.n_bitrates = ARRAY_SIZE(wfx_rates),
+	.ht_cap = {
+		// Receive caps
+		.cap = IEEE80211_HT_CAP_GRN_FLD | IEEE80211_HT_CAP_SGI_20 |
+		       IEEE80211_HT_CAP_MAX_AMSDU | (1 << IEEE80211_HT_CAP_RX_STBC_SHIFT),
+		.ht_supported = 1,
+		.ampdu_factor = IEEE80211_HT_MAX_AMPDU_16K,
+		.ampdu_density = IEEE80211_HT_MPDU_DENSITY_NONE,
+		.mcs = {
+			.rx_mask = { 0xFF }, // MCS0 to MCS7
+			.rx_highest = 65,
+			.tx_params = IEEE80211_HT_MCS_TX_DEFINED,
+		},
+	},
+};
+
+static const struct ieee80211_iface_limit wdev_iface_limits[] = {
+	{ .max = 1, .types = BIT(NL80211_IFTYPE_STATION) },
+	{ .max = 1, .types = BIT(NL80211_IFTYPE_AP) },
+};
+
+static const struct ieee80211_iface_combination wfx_iface_combinations[] = {
+	{
+		.num_different_channels = 2,
+		.max_interfaces = 2,
+		.limits = wdev_iface_limits,
+		.n_limits = ARRAY_SIZE(wdev_iface_limits),
+	}
+};
+
 static const struct ieee80211_ops wfx_ops = {
 	.start			= wfx_start,
 	.stop			= wfx_stop,
 	.add_interface		= wfx_add_interface,
 	.remove_interface	= wfx_remove_interface,
+	.config			= wfx_config,
 	.tx			= wfx_tx,
+	.conf_tx		= wfx_conf_tx,
 	.hw_scan		= wfx_hw_scan,
+	.sta_add		= wfx_sta_add,
+	.sta_remove		= wfx_sta_remove,
+	.sta_notify		= wfx_sta_notify,
+	.set_tim		= wfx_set_tim,
 	.set_key		= wfx_set_key,
+	.set_rts_threshold	= wfx_set_rts_threshold,
+	.bss_info_changed	= wfx_bss_info_changed,
+	.prepare_multicast	= wfx_prepare_multicast,
+	.configure_filter	= wfx_configure_filter,
+	.ampdu_action		= wfx_ampdu_action,
+	.flush			= wfx_flush,
+	.add_chanctx		= wfx_add_chanctx,
+	.remove_chanctx		= wfx_remove_chanctx,
+	.change_chanctx		= wfx_change_chanctx,
+	.assign_vif_chanctx	= wfx_assign_vif_chanctx,
+	.unassign_vif_chanctx	= wfx_unassign_vif_chanctx,
 };
 
 bool wfx_api_older_than(struct wfx_dev *wdev, int major, int minor)
@@ -198,6 +296,16 @@ struct wfx_dev *wfx_init_common(struct device *dev,
 
 	SET_IEEE80211_DEV(hw, dev);
 
+	ieee80211_hw_set(hw, NEED_DTIM_BEFORE_ASSOC);
+	ieee80211_hw_set(hw, TX_AMPDU_SETUP_IN_HW);
+	ieee80211_hw_set(hw, AMPDU_AGGREGATION);
+	ieee80211_hw_set(hw, CONNECTION_MONITOR);
+	ieee80211_hw_set(hw, REPORTS_TX_ACK_STATUS);
+	ieee80211_hw_set(hw, SUPPORTS_DYNAMIC_PS);
+	ieee80211_hw_set(hw, SIGNAL_DBM);
+	ieee80211_hw_set(hw, SUPPORTS_PS);
+	ieee80211_hw_set(hw, MFP_CAPABLE);
+
 	hw->vif_data_size = sizeof(struct wfx_vif);
 	hw->sta_data_size = sizeof(struct wfx_sta_priv);
 	hw->queues = 4;
@@ -206,8 +314,19 @@ struct wfx_dev *wfx_init_common(struct device *dev,
 	hw->extra_tx_headroom = sizeof(struct hif_sl_msg_hdr) + sizeof(struct hif_msg)
 				+ sizeof(struct hif_req_tx)
 				+ 4 /* alignment */ + 8 /* TKIP IV */;
+	hw->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
+				     BIT(NL80211_IFTYPE_ADHOC) |
+				     BIT(NL80211_IFTYPE_AP);
+	hw->wiphy->flags |= WIPHY_FLAG_AP_UAPSD;
+	hw->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
+	hw->wiphy->max_ap_assoc_sta = WFX_MAX_STA_IN_AP_MODE;
 	hw->wiphy->max_scan_ssids = 2;
 	hw->wiphy->max_scan_ie_len = IEEE80211_MAX_DATA_LEN;
+	hw->wiphy->n_iface_combinations = ARRAY_SIZE(wfx_iface_combinations);
+	hw->wiphy->iface_combinations = wfx_iface_combinations;
+	hw->wiphy->bands[NL80211_BAND_2GHZ] = devm_kmalloc(dev, sizeof(wfx_band_2ghz), GFP_KERNEL);
+	// FIXME: also copy wfx_rates and wfx_2ghz_chantable
+	memcpy(hw->wiphy->bands[NL80211_BAND_2GHZ], &wfx_band_2ghz, sizeof(wfx_band_2ghz));
 
 	wdev = hw->priv;
 	wdev->hw = hw;
@@ -290,6 +409,12 @@ int wfx_probe(struct wfx_dev *wdev)
 		goto err1;
 	}
 
+	if (wdev->hw_caps.regul_sel_mode_info.region_sel_mode) {
+		wdev->hw->wiphy->bands[NL80211_BAND_2GHZ]->channels[11].flags |= IEEE80211_CHAN_NO_IR;
+		wdev->hw->wiphy->bands[NL80211_BAND_2GHZ]->channels[12].flags |= IEEE80211_CHAN_NO_IR;
+		wdev->hw->wiphy->bands[NL80211_BAND_2GHZ]->channels[13].flags |= IEEE80211_CHAN_DISABLED;
+	}
+
 	dev_dbg(wdev->dev, "sending configuration file %s\n", wdev->pdata.file_pds);
 	err = wfx_send_pdata_pds(wdev);
 	if (err < 0)
@@ -322,6 +447,12 @@ int wfx_probe(struct wfx_dev *wdev)
 		}
 		dev_info(wdev->dev, "MAC address %d: %pM\n", i, wdev->addresses[i].addr);
 	}
+	wdev->hw->wiphy->n_addresses = ARRAY_SIZE(wdev->addresses);
+	wdev->hw->wiphy->addresses = wdev->addresses;
+
+	err = ieee80211_register_hw(wdev->hw);
+	if (err)
+		goto err1;
 
 	err = wfx_debug_init(wdev);
 	if (err)
@@ -330,6 +461,7 @@ int wfx_probe(struct wfx_dev *wdev)
 	return 0;
 
 err2:
+	ieee80211_unregister_hw(wdev->hw);
 	ieee80211_free_hw(wdev->hw);
 err1:
 	wfx_bh_unregister(wdev);
@@ -338,6 +470,7 @@ err1:
 
 void wfx_release(struct wfx_dev *wdev)
 {
+	ieee80211_unregister_hw(wdev->hw);
 	hif_shutdown(wdev);
 	wfx_bh_unregister(wdev);
 	wfx_sl_deinit(wdev);
diff --git a/drivers/staging/wfx/queue.c b/drivers/staging/wfx/queue.c
index aa438be21d37..6f1be4f6f463 100644
--- a/drivers/staging/wfx/queue.c
+++ b/drivers/staging/wfx/queue.c
@@ -351,6 +351,83 @@ bool wfx_tx_queues_is_empty(struct wfx_dev *wdev)
 	return ret;
 }
 
+static bool hif_handle_tx_data(struct wfx_vif *wvif, struct sk_buff *skb,
+			       struct wfx_queue *queue)
+{
+	bool handled = false;
+	struct wfx_tx_priv *tx_priv = wfx_skb_tx_priv(skb);
+	struct hif_req_tx *req = wfx_skb_txreq(skb);
+	struct ieee80211_hdr *frame = (struct ieee80211_hdr *) (req->frame + req->data_flags.fc_offset);
+
+	enum {
+		do_probe,
+		do_drop,
+		do_wep,
+		do_tx,
+	} action = do_tx;
+
+	switch (wvif->vif->type) {
+	case NL80211_IFTYPE_STATION:
+		if (wvif->state < WFX_STATE_PRE_STA)
+			action = do_drop;
+		break;
+	case NL80211_IFTYPE_AP:
+		if (!wvif->state) {
+			action = do_drop;
+		} else if (!(BIT(tx_priv->raw_link_id) & (BIT(0) | wvif->link_id_map))) {
+			dev_warn(wvif->wdev->dev, "a frame with expired link-id is dropped\n");
+			action = do_drop;
+		}
+		break;
+	case NL80211_IFTYPE_ADHOC:
+		if (wvif->state != WFX_STATE_IBSS)
+			action = do_drop;
+		break;
+	case NL80211_IFTYPE_MONITOR:
+	default:
+		action = do_drop;
+		break;
+	}
+
+	if (action == do_tx) {
+		if (ieee80211_is_nullfunc(frame->frame_control)) {
+			mutex_lock(&wvif->bss_loss_lock);
+			if (wvif->bss_loss_state) {
+				wvif->bss_loss_confirm_id = req->packet_id;
+				req->queue_id.queue_id = HIF_QUEUE_ID_VOICE;
+			}
+			mutex_unlock(&wvif->bss_loss_lock);
+		} else if (ieee80211_has_protected(frame->frame_control) &&
+			   tx_priv->hw_key &&
+			   tx_priv->hw_key->keyidx != wvif->wep_default_key_id &&
+			   (tx_priv->hw_key->cipher == WLAN_CIPHER_SUITE_WEP40 ||
+			    tx_priv->hw_key->cipher == WLAN_CIPHER_SUITE_WEP104)) {
+			action = do_wep;
+		}
+	}
+
+	switch (action) {
+	case do_drop:
+		BUG_ON(wfx_pending_remove(wvif->wdev, skb));
+		handled = true;
+		break;
+	case do_wep:
+		wfx_tx_lock(wvif->wdev);
+		wvif->wep_default_key_id = tx_priv->hw_key->keyidx;
+		wvif->wep_pending_skb = skb;
+		if (!schedule_work(&wvif->wep_key_work))
+			wfx_tx_unlock(wvif->wdev);
+		handled = true;
+		break;
+	case do_tx:
+		break;
+	default:
+		/* Do nothing */
+		break;
+	}
+	return handled;
+}
+
 static int wfx_get_prio_queue(struct wfx_vif *wvif,
 				 u32 tx_allowed_mask, int *total)
 {
@@ -498,6 +575,9 @@ struct hif_msg *wfx_tx_queues_get(struct wfx_dev *wdev)
 		wvif = wdev_to_wvif(wdev, hif->interface);
 		WARN_ON(!wvif);
 
+		if (hif_handle_tx_data(wvif, skb, queue))
+			continue;  /* Handled by WSM */
+
 		wvif->pspoll_mask &= ~BIT(tx_priv->raw_link_id);
 
 		/* allow bursting if txop is set */
diff --git a/drivers/staging/wfx/scan.c b/drivers/staging/wfx/scan.c
index 207b26ebc9fd..ea5001c915f6 100644
--- a/drivers/staging/wfx/scan.c
+++ b/drivers/staging/wfx/scan.c
@@ -21,11 +21,26 @@ static void __ieee80211_scan_completed_compat(struct ieee80211_hw *hw, bool abor
 	ieee80211_scan_completed(hw, &info);
 }
 
+static void wfx_scan_restart_delayed(struct wfx_vif *wvif)
+{
+	if (wvif->delayed_unjoin) {
+		wvif->delayed_unjoin = false;
+		if (!schedule_work(&wvif->unjoin_work))
+			wfx_tx_unlock(wvif->wdev);
+	} else if (wvif->delayed_link_loss) {
+		wvif->delayed_link_loss = 0;
+		wfx_cqm_bssloss_sm(wvif, 1, 0, 0);
+	}
+}
+
 static int wfx_scan_start(struct wfx_vif *wvif, struct wfx_scan_params *scan)
 {
 	int ret;
 	int tmo = 500;
 
+	if (wvif->state == WFX_STATE_PRE_STA)
+		return -EBUSY;
+
 	tmo += scan->scan_req.num_of_channels *
 	       ((20 * (scan->scan_req.max_channel_time)) + 10);
 	atomic_set(&wvif->scan.in_progress, 1);
@@ -38,6 +53,7 @@ static int wfx_scan_start(struct wfx_vif *wvif, struct wfx_scan_params *scan)
 		atomic_set(&wvif->scan.in_progress, 0);
 		atomic_set(&wvif->wdev->scan_in_progress, 0);
 		cancel_delayed_work_sync(&wvif->scan.timeout);
+		wfx_scan_restart_delayed(wvif);
 	}
 	return ret;
 }
@@ -56,6 +72,9 @@ int wfx_hw_scan(struct ieee80211_hw *hw,
 	if (!wvif)
 		return -EINVAL;
 
+	if (wvif->state == WFX_STATE_AP)
+		return -EOPNOTSUPP;
+
 	if (req->n_ssids == 1 && !req->ssids[0].ssid_len)
 		req->n_ssids = 0;
 
@@ -121,11 +140,23 @@ void wfx_scan_work(struct work_struct *work)
 		.scan_req.scan_type.type = 0,    /* Foreground */
 	};
 	struct ieee80211_channel *first;
+	bool first_run = (wvif->scan.begin == wvif->scan.curr &&
+			  wvif->scan.begin != wvif->scan.end);
 	int i;
 
 	down(&wvif->scan.lock);
 	mutex_lock(&wvif->wdev->conf_mutex);
 
+	if (first_run) {
+		if (wvif->state == WFX_STATE_STA &&
+		    !(wvif->powersave_mode.pm_mode.enter_psm)) {
+			struct hif_req_set_pm_mode pm = wvif->powersave_mode;
+
+			pm.pm_mode.enter_psm = 1;
+			wfx_set_pm(wvif, &pm);
+		}
+	}
+
 	if (!wvif->scan.req || wvif->scan.curr == wvif->scan.end) {
 		if (wvif->scan.output_power != wvif->wdev->output_power)
 			hif_set_output_power(wvif, wvif->wdev->output_power * 10);
@@ -138,10 +169,14 @@ void wfx_scan_work(struct work_struct *work)
 			dev_dbg(wvif->wdev->dev, "scan canceled\n");
 
 		wvif->scan.req = NULL;
+		wfx_scan_restart_delayed(wvif);
 		wfx_tx_unlock(wvif->wdev);
 		mutex_unlock(&wvif->wdev->conf_mutex);
 		__ieee80211_scan_completed_compat(wvif->wdev->hw, wvif->scan.status ? 1 : 0);
 		up(&wvif->scan.lock);
+		if (wvif->state == WFX_STATE_STA &&
+		    !(wvif->powersave_mode.pm_mode.enter_psm))
+			wfx_set_pm(wvif, &wvif->powersave_mode);
 		return;
 	}
 	first = *wvif->scan.curr;
@@ -170,6 +205,11 @@ void wfx_scan_work(struct work_struct *work)
 	scan.ssids = &wvif->scan.ssids[0];
 	scan.scan_req.num_of_channels = it - wvif->scan.curr;
 	scan.scan_req.probe_delay = 100;
+	// FIXME: Check if FW can do active scan while joined.
+	if (wvif->state == WFX_STATE_STA) {
+		scan.scan_req.scan_type.type = 1;
+		scan.scan_req.scan_flags.fbg = 1;
+	}
 
 	scan.ch = kcalloc(scan.scan_req.num_of_channels, sizeof(u8), GFP_KERNEL);
 
diff --git a/drivers/staging/wfx/sta.c b/drivers/staging/wfx/sta.c
index 2e709b8a3bf4..2855d14a709c 100644
--- a/drivers/staging/wfx/sta.c
+++ b/drivers/staging/wfx/sta.c
@@ -9,11 +9,148 @@
 
 #include "sta.h"
 #include "wfx.h"
+#include "fwio.h"
+#include "bh.h"
 #include "key.h"
 #include "scan.h"
+#include "debug.h"
+#include "hif_tx.h"
 #include "hif_tx_mib.h"
 
 #define TXOP_UNIT 32
+#define HIF_MAX_ARP_IP_ADDRTABLE_ENTRIES 2
+
+static u32 wfx_rate_mask_to_hw(struct wfx_dev *wdev, u32 rates)
+{
+	int i;
+	u32 ret = 0;
+	// WFx only support 2GHz
+	struct ieee80211_supported_band *sband = wdev->hw->wiphy->bands[NL80211_BAND_2GHZ];
+
+	for (i = 0; i < sband->n_bitrates; i++) {
+		if (rates & BIT(i)) {
+			if (i >= sband->n_bitrates)
+				dev_warn(wdev->dev, "unsupported basic rate\n");
+			else
+				ret |= BIT(sband->bitrates[i].hw_value);
+		}
+	}
+	return ret;
+}
+
+static void __wfx_free_event_queue(struct list_head *list)
+{
+	struct wfx_hif_event *event, *tmp;
+
+	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);
+}
+
+void wfx_cqm_bssloss_sm(struct wfx_vif *wvif, int init, int good, int bad)
+{
+	int tx = 0;
+
+	mutex_lock(&wvif->bss_loss_lock);
+	wvif->delayed_link_loss = 0;
+	cancel_work_sync(&wvif->bss_params_work);
+
+	/* If we have a pending unjoin */
+	if (wvif->delayed_unjoin)
+		goto end;
+
+	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;
+	} 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;
+
+		wvif->bss_loss_state++;
+
+		skb = ieee80211_nullfunc_get(wvif->wdev->hw, wvif->vif, false);
+		if (!skb)
+			goto end;
+		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;
+		wfx_tx(wvif->wdev->hw, NULL, skb);
+	}
+end:
+	mutex_unlock(&wvif->bss_loss_lock);
+}
+
+static int wfx_set_uapsd_param(struct wfx_vif *wvif,
+			   const struct wfx_edca_params *arg)
+{
+	int ret;
+
+	/* Here's the mapping AC [queue, bit]
+	 *  VO [0,3], VI [1, 2], BE [2, 1], BK [3, 0]
+	 */
+
+	if (arg->uapsd_enable[IEEE80211_AC_VO])
+		wvif->uapsd_info.trig_voice = 1;
+	else
+		wvif->uapsd_info.trig_voice = 0;
+
+	if (arg->uapsd_enable[IEEE80211_AC_VI])
+		wvif->uapsd_info.trig_video = 1;
+	else
+		wvif->uapsd_info.trig_video = 0;
+
+	if (arg->uapsd_enable[IEEE80211_AC_BE])
+		wvif->uapsd_info.trig_be = 1;
+	else
+		wvif->uapsd_info.trig_be = 0;
+
+	if (arg->uapsd_enable[IEEE80211_AC_BK])
+		wvif->uapsd_info.trig_bckgrnd = 1;
+	else
+		wvif->uapsd_info.trig_bckgrnd = 0;
+
+	/* Currently pseudo U-APSD operation is not supported, so setting
+	 * MinAutoTriggerInterval, MaxAutoTriggerInterval and
+	 * AutoTriggerStep to 0
+	 */
+	wvif->uapsd_info.min_auto_trigger_interval = 0;
+	wvif->uapsd_info.max_auto_trigger_interval = 0;
+	wvif->uapsd_info.auto_trigger_step = 0;
+
+	ret = hif_set_uapsd_info(wvif, &wvif->uapsd_info);
+	return ret;
+}
 
 int wfx_fwd_probe_req(struct wfx_vif *wvif, bool enable)
 {
@@ -22,6 +159,1044 @@ int wfx_fwd_probe_req(struct wfx_vif *wvif, bool enable)
 				 wvif->fwd_probe_req);
 }
 
+static int wfx_set_mcast_filter(struct wfx_vif *wvif,
+				    struct wfx_grp_addr_table *fp)
+{
+	int i, ret;
+	struct hif_mib_config_data_filter config = { };
+	struct hif_mib_set_data_filtering filter_data = { };
+	struct hif_mib_mac_addr_data_frame_condition filter_addr_val = { };
+	struct hif_mib_uc_mc_bc_data_frame_condition filter_addr_type = { };
+
+	// Temporary workaround for filters
+	return hif_set_data_filtering(wvif, &filter_data);
+
+	if (!fp->enable) {
+		filter_data.enable = 0;
+		return hif_set_data_filtering(wvif, &filter_data);
+	}
+
+	// A1 Address match on list
+	for (i = 0; i < fp->num_addresses; i++) {
+		filter_addr_val.condition_idx = i;
+		filter_addr_val.address_type = HIF_MAC_ADDR_A1;
+		ether_addr_copy(filter_addr_val.mac_address, fp->address_list[i]);
+		ret = hif_set_mac_addr_condition(wvif, &filter_addr_val);
+		if (ret)
+			return ret;
+		config.mac_cond |= 1 << i;
+	}
+
+	// Accept unicast and broadcast
+	filter_addr_type.condition_idx = 0;
+	filter_addr_type.param.bits.type_unicast = 1;
+	filter_addr_type.param.bits.type_broadcast = 1;
+	ret = hif_set_uc_mc_bc_condition(wvif, &filter_addr_type);
+	if (ret)
+		return ret;
+
+	config.uc_mc_bc_cond = 1;
+	config.filter_idx = 0; // TODO #define MULTICAST_FILTERING 0
+	config.enable = 1;
+	ret = hif_set_config_data_filter(wvif, &config);
+	if (ret)
+		return ret;
+
+	// discard all data frames except match filter
+	filter_data.enable = 1;
+	filter_data.default_filter = 1; // discard all
+	ret = hif_set_data_filtering(wvif, &filter_data);
+
+	return ret;
+}
+
+void wfx_update_filtering(struct wfx_vif *wvif)
+{
+	int ret;
+	bool is_sta = wvif->vif && NL80211_IFTYPE_STATION == wvif->vif->type;
+	bool filter_bssid = wvif->filter_bssid;
+	bool fwd_probe_req = wvif->fwd_probe_req;
+	struct hif_mib_bcn_filter_enable bf_ctrl;
+	struct hif_mib_bcn_filter_table *bf_tbl;
+	struct hif_ie_table_entry ie_tbl[] = {
+		{
+			.ie_id        = WLAN_EID_VENDOR_SPECIFIC,
+			.has_changed  = 1,
+			.no_longer    = 1,
+			.has_appeared = 1,
+			.oui         = { 0x50, 0x6F, 0x9A},
+		}, {
+			.ie_id        = WLAN_EID_HT_OPERATION,
+			.has_changed  = 1,
+			.no_longer    = 1,
+			.has_appeared = 1,
+		}, {
+			.ie_id        = WLAN_EID_ERP_INFO,
+			.has_changed  = 1,
+			.no_longer    = 1,
+			.has_appeared = 1,
+		}
+	};
+
+	if (wvif->state == WFX_STATE_PASSIVE)
+		return;
+
+	bf_tbl = kmalloc(sizeof(struct hif_mib_bcn_filter_table) + sizeof(ie_tbl), GFP_KERNEL);
+	memcpy(bf_tbl->ie_table, ie_tbl, sizeof(ie_tbl));
+	if (wvif->disable_beacon_filter) {
+		bf_ctrl.enable = 0;
+		bf_ctrl.bcn_count = 1;
+		bf_tbl->num_of_info_elmts = 0;
+	} else if (!is_sta) {
+		bf_ctrl.enable = HIF_BEACON_FILTER_ENABLE | HIF_BEACON_FILTER_AUTO_ERP;
+		bf_ctrl.bcn_count = 0;
+		bf_tbl->num_of_info_elmts = 2;
+	} else {
+		bf_ctrl.enable = HIF_BEACON_FILTER_ENABLE;
+		bf_ctrl.bcn_count = 0;
+		bf_tbl->num_of_info_elmts = 3;
+	}
+
+	ret = hif_set_rx_filter(wvif, filter_bssid, fwd_probe_req);
+	if (!ret)
+		ret = hif_set_beacon_filter_table(wvif, bf_tbl);
+	if (!ret)
+		ret = hif_beacon_filter_control(wvif, bf_ctrl.enable, bf_ctrl.bcn_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);
+	kfree(bf_tbl);
+}
+
+void wfx_update_filtering_work(struct work_struct *work)
+{
+	struct wfx_vif *wvif = container_of(work, struct wfx_vif, update_filtering_work);
+
+	wfx_update_filtering(wvif);
+}
+
+u64 wfx_prepare_multicast(struct ieee80211_hw *hw, struct netdev_hw_addr_list *mc_list)
+{
+	int i;
+	struct netdev_hw_addr *ha;
+	struct wfx_vif *wvif = NULL;
+	struct wfx_dev *wdev = hw->priv;
+	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))
+			continue;
+
+		i = 0;
+		netdev_hw_addr_list_for_each(ha, mc_list) {
+			ether_addr_copy(wvif->mcast_filter.address_list[i], ha->addr);
+			i++;
+		}
+		wvif->mcast_filter.enable = 1;
+		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)
+{
+	struct wfx_vif *wvif = NULL;
+	struct wfx_dev *wdev = hw->priv;
+
+	*total_flags &= FIF_OTHER_BSS | FIF_FCSFAIL | FIF_PROBE_REQ;
+
+	while ((wvif = wvif_iterate(wdev, wvif)) != NULL) {
+		down(&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);
+		up(&wvif->scan.lock);
+	}
+}
+
+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;
+	int ret = 0;
+	/* To prevent re-applying PM request OID again and again*/
+	u16 old_uapsd_flags, new_uapsd_flags;
+	struct hif_req_edca_queue_params *edca;
+
+	mutex_lock(&wdev->conf_mutex);
+
+	if (queue < hw->queues) {
+		old_uapsd_flags = *((u16 *) &wvif->uapsd_info);
+		edca = &wvif->edca.params[queue];
+
+		wvif->edca.uapsd_enable[queue] = params->uapsd;
+		edca->aifsn = params->aifs;
+		edca->cw_min = params->cw_min;
+		edca->cw_max = params->cw_max;
+		edca->tx_op_limit = params->txop * TXOP_UNIT;
+		edca->allowed_medium_time = 0;
+		ret = hif_set_edca_queue_params(wvif, edca);
+		if (ret) {
+			ret = -EINVAL;
+			goto out;
+		}
+
+		if (wvif->vif->type == NL80211_IFTYPE_STATION) {
+			ret = wfx_set_uapsd_param(wvif, &wvif->edca);
+			new_uapsd_flags = *((u16 *) &wvif->uapsd_info);
+			if (!ret && wvif->setbssparams_done &&
+			    wvif->state == WFX_STATE_STA &&
+			    old_uapsd_flags != new_uapsd_flags)
+				ret = wfx_set_pm(wvif, &wvif->powersave_mode);
+		}
+	} else {
+		ret = -EINVAL;
+	}
+
+out:
+	mutex_unlock(&wdev->conf_mutex);
+	return ret;
+}
+
+int wfx_set_pm(struct wfx_vif *wvif, const struct hif_req_set_pm_mode *arg)
+{
+	struct hif_req_set_pm_mode pm = *arg;
+	u16 uapsd_flags;
+	int ret;
+
+	if (wvif->state != WFX_STATE_STA || !wvif->bss_params.aid)
+		return 0;
+
+	memcpy(&uapsd_flags, &wvif->uapsd_info, sizeof(uapsd_flags));
+
+	if (uapsd_flags != 0)
+		pm.pm_mode.fast_psm = 0;
+
+	// Kernel disable PowerSave when multiple vifs are in use. In contrary,
+	// it is absolutly necessary to enable PowerSave for WF200
+	if (wvif_count(wvif->wdev) > 1) {
+		pm.pm_mode.enter_psm = 1;
+		pm.pm_mode.fast_psm = 0;
+	}
+
+	if (!wait_for_completion_timeout(&wvif->set_pm_mode_complete, msecs_to_jiffies(300)))
+		dev_warn(wvif->wdev->dev, "timeout while waiting of set_pm_mode_complete\n");
+	ret = hif_set_pm(wvif, &pm);
+	// FIXME: why ?
+	if (-ETIMEDOUT == wvif->scan.stat