[PATCH v2 18/20] staging: wfx: allow to scan networks

[Jerome Pouiller]

From: JÃrÃme Pouiller <jerome.pouiller@xxxxxxxxxx>

Signed-off-by: JÃrÃme Pouiller <jerome.pouiller@xxxxxxxxxx>
---
 drivers/staging/wfx/Makefile |   1 +
 drivers/staging/wfx/bh.c     |   2 +-
 drivers/staging/wfx/hif_rx.c |  13 ++
 drivers/staging/wfx/main.c   |   5 +
 drivers/staging/wfx/scan.c   | 249 +++++++++++++++++++++++++++++++++++
 drivers/staging/wfx/scan.h   |  42 ++++++
 drivers/staging/wfx/sta.c    |  23 +++-
 drivers/staging/wfx/sta.h    |   4 +
 drivers/staging/wfx/wfx.h    |  11 ++
 9 files changed, 348 insertions(+), 2 deletions(-)
 create mode 100644 drivers/staging/wfx/scan.c
 create mode 100644 drivers/staging/wfx/scan.h

diff --git a/drivers/staging/wfx/Makefile b/drivers/staging/wfx/Makefile
index d9e21515d08e..2b8a5fa86fac 100644
--- a/drivers/staging/wfx/Makefile
+++ b/drivers/staging/wfx/Makefile
@@ -12,6 +12,7 @@ wfx-y := \
 	queue.o \
 	data_tx.o \
 	data_rx.o \
+	scan.o \
 	sta.o \
 	main.o \
 	sta.o \
diff --git a/drivers/staging/wfx/bh.c b/drivers/staging/wfx/bh.c
index ed81c3924d98..6000c03bb658 100644
--- a/drivers/staging/wfx/bh.c
+++ b/drivers/staging/wfx/bh.c
@@ -268,7 +268,7 @@ static void bh_work(struct work_struct *work)
 
 	if (last_op_is_rx)
 		ack_sdio_data(wdev);
-	if (!wdev->hif.tx_buffers_used && !work_pending(work)) {
+	if (!wdev->hif.tx_buffers_used && !work_pending(work) && !atomic_read(&wdev->scan_in_progress)) {
 		device_release(wdev);
 		release_chip = true;
 	}
diff --git a/drivers/staging/wfx/hif_rx.c b/drivers/staging/wfx/hif_rx.c
index c07984b0535d..d386fab0a90f 100644
--- a/drivers/staging/wfx/hif_rx.c
+++ b/drivers/staging/wfx/hif_rx.c
@@ -11,6 +11,7 @@
 
 #include "hif_rx.h"
 #include "wfx.h"
+#include "scan.h"
 #include "data_rx.h"
 #include "secure_link.h"
 #include "hif_api_cmd.h"
@@ -143,6 +144,17 @@ static int hif_receive_indication(struct wfx_dev *wdev, struct hif_msg *hif, voi
 	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);
+	struct hif_ind_scan_cmpl *body = buf;
+
+	WARN_ON(!wvif);
+	wfx_scan_complete_cb(wvif, body);
+
+	return 0;
+}
+
 static int hif_join_complete_indication(struct wfx_dev *wdev, struct hif_msg *hif, void *buf)
 {
 	struct wfx_vif *wvif = wdev_to_wvif(wdev, hif->interface);
@@ -230,6 +242,7 @@ 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_SCAN_CMPL,            hif_scan_complete_indication },
 	{ HIF_IND_ID_SL_EXCHANGE_PUB_KEYS, hif_keys_indication },
 	{ HIF_IND_ID_GENERIC,              hif_generic_indication },
 	{ HIF_IND_ID_ERROR,                hif_error_indication },
diff --git a/drivers/staging/wfx/main.c b/drivers/staging/wfx/main.c
index cce4e30dd94a..06220bac5b75 100644
--- a/drivers/staging/wfx/main.c
+++ b/drivers/staging/wfx/main.c
@@ -55,6 +55,7 @@ static const struct ieee80211_ops wfx_ops = {
 	.add_interface		= wfx_add_interface,
 	.remove_interface	= wfx_remove_interface,
 	.tx			= wfx_tx,
+	.hw_scan		= wfx_hw_scan,
 };
 
 bool wfx_api_older_than(struct wfx_dev *wdev, int major, int minor)
@@ -203,6 +204,8 @@ 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->max_scan_ssids = 2;
+	hw->wiphy->max_scan_ie_len = IEEE80211_MAX_DATA_LEN;
 
 	wdev = hw->priv;
 	wdev->hw = hw;
@@ -214,6 +217,7 @@ struct wfx_dev *wfx_init_common(struct device *dev,
 	wdev->pdata.gpio_wakeup = wfx_get_gpio(dev, gpio_wakeup, "wakeup");
 	wfx_fill_sl_key(dev, &wdev->pdata);
 
+	mutex_init(&wdev->conf_mutex);
 	mutex_init(&wdev->rx_stats_lock);
 	init_completion(&wdev->firmware_ready);
 	wfx_init_hif_cmd(&wdev->hif_cmd);
@@ -225,6 +229,7 @@ struct wfx_dev *wfx_init_common(struct device *dev,
 void wfx_free_common(struct wfx_dev *wdev)
 {
 	mutex_destroy(&wdev->rx_stats_lock);
+	mutex_destroy(&wdev->conf_mutex);
 	wfx_tx_queues_deinit(wdev);
 	ieee80211_free_hw(wdev->hw);
 }
diff --git a/drivers/staging/wfx/scan.c b/drivers/staging/wfx/scan.c
new file mode 100644
index 000000000000..207b26ebc9fd
--- /dev/null
+++ b/drivers/staging/wfx/scan.c
@@ -0,0 +1,249 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Scan related functions.
+ *
+ * Copyright (c) 2017-2019, Silicon Laboratories, Inc.
+ * Copyright (c) 2010, ST-Ericsson
+ */
+#include <net/mac80211.h>
+
+#include "scan.h"
+#include "wfx.h"
+#include "sta.h"
+#include "hif_tx_mib.h"
+
+static void __ieee80211_scan_completed_compat(struct ieee80211_hw *hw, bool aborted)
+{
+	struct cfg80211_scan_info info = {
+		.aborted = aborted ? 1 : 0,
+	};
+
+	ieee80211_scan_completed(hw, &info);
+}
+
+static int wfx_scan_start(struct wfx_vif *wvif, struct wfx_scan_params *scan)
+{
+	int ret;
+	int tmo = 500;
+
+	tmo += scan->scan_req.num_of_channels *
+	       ((20 * (scan->scan_req.max_channel_time)) + 10);
+	atomic_set(&wvif->scan.in_progress, 1);
+	atomic_set(&wvif->wdev->scan_in_progress, 1);
+
+	schedule_delayed_work(&wvif->scan.timeout, msecs_to_jiffies(tmo));
+	ret = hif_scan(wvif, scan);
+	if (ret) {
+		wfx_scan_failed_cb(wvif);
+		atomic_set(&wvif->scan.in_progress, 0);
+		atomic_set(&wvif->wdev->scan_in_progress, 0);
+		cancel_delayed_work_sync(&wvif->scan.timeout);
+	}
+	return ret;
+}
+
+int wfx_hw_scan(struct ieee80211_hw *hw,
+		   struct ieee80211_vif *vif,
+		   struct ieee80211_scan_request *hw_req)
+{
+	struct wfx_dev *wdev = hw->priv;
+	struct wfx_vif *wvif = (struct wfx_vif *) vif->drv_priv;
+	struct cfg80211_scan_request *req = &hw_req->req;
+	struct sk_buff *skb;
+	int i, ret;
+	struct hif_mib_template_frame *p;
+
+	if (!wvif)
+		return -EINVAL;
+
+	if (req->n_ssids == 1 && !req->ssids[0].ssid_len)
+		req->n_ssids = 0;
+
+	if (req->n_ssids > HIF_API_MAX_NB_SSIDS)
+		return -EINVAL;
+
+	skb = ieee80211_probereq_get(hw, wvif->vif->addr, NULL, 0, req->ie_len);
+	if (!skb)
+		return -ENOMEM;
+
+	if (req->ie_len)
+		memcpy(skb_put(skb, req->ie_len), req->ie, req->ie_len);
+
+	mutex_lock(&wdev->conf_mutex);
+
+	p = (struct hif_mib_template_frame *)skb_push(skb, 4);
+	p->frame_type = HIF_TMPLT_PRBREQ;
+	p->frame_length = cpu_to_le16(skb->len - 4);
+	ret = hif_set_template_frame(wvif, p);
+	skb_pull(skb, 4);
+
+	if (!ret)
+		/* Host want to be the probe responder. */
+		ret = wfx_fwd_probe_req(wvif, true);
+	if (ret) {
+		mutex_unlock(&wdev->conf_mutex);
+		dev_kfree_skb(skb);
+		return ret;
+	}
+
+	wfx_tx_lock_flush(wdev);
+
+	BUG_ON(wvif->scan.req);
+	wvif->scan.req = req;
+	wvif->scan.n_ssids = 0;
+	wvif->scan.status = 0;
+	wvif->scan.begin = &req->channels[0];
+	wvif->scan.curr = wvif->scan.begin;
+	wvif->scan.end = &req->channels[req->n_channels];
+	wvif->scan.output_power = wdev->output_power;
+
+	for (i = 0; i < req->n_ssids; ++i) {
+		struct hif_ssid_def *dst = &wvif->scan.ssids[wvif->scan.n_ssids];
+
+		memcpy(&dst->ssid[0], req->ssids[i].ssid, sizeof(dst->ssid));
+		dst->ssid_length = req->ssids[i].ssid_len;
+		++wvif->scan.n_ssids;
+	}
+
+	mutex_unlock(&wdev->conf_mutex);
+
+	if (skb)
+		dev_kfree_skb(skb);
+	schedule_work(&wvif->scan.work);
+	return 0;
+}
+
+void wfx_scan_work(struct work_struct *work)
+{
+	struct wfx_vif *wvif = container_of(work, struct wfx_vif, scan.work);
+	struct ieee80211_channel **it;
+	struct wfx_scan_params scan = {
+		.scan_req.scan_type.type = 0,    /* Foreground */
+	};
+	struct ieee80211_channel *first;
+	int i;
+
+	down(&wvif->scan.lock);
+	mutex_lock(&wvif->wdev->conf_mutex);
+
+	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);
+
+		if (wvif->scan.status < 0)
+			dev_warn(wvif->wdev->dev, "scan failed\n");
+		else if (wvif->scan.req)
+			dev_dbg(wvif->wdev->dev, "scan completed\n");
+		else
+			dev_dbg(wvif->wdev->dev, "scan canceled\n");
+
+		wvif->scan.req = NULL;
+		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);
+		return;
+	}
+	first = *wvif->scan.curr;
+
+	for (it = wvif->scan.curr + 1, i = 1;
+	     it != wvif->scan.end && i < HIF_API_MAX_NB_CHANNELS;
+	     ++it, ++i) {
+		if ((*it)->band != first->band)
+			break;
+		if (((*it)->flags ^ first->flags) &
+				IEEE80211_CHAN_NO_IR)
+			break;
+		if (!(first->flags & IEEE80211_CHAN_NO_IR) &&
+		    (*it)->max_power != first->max_power)
+			break;
+	}
+	scan.scan_req.band = first->band;
+
+	if (wvif->scan.req->no_cck)
+		scan.scan_req.max_transmit_rate = API_RATE_INDEX_G_6MBPS;
+	else
+		scan.scan_req.max_transmit_rate = API_RATE_INDEX_B_1MBPS;
+	scan.scan_req.num_of_probe_requests =
+		(first->flags & IEEE80211_CHAN_NO_IR) ? 0 : 2;
+	scan.scan_req.num_of_ssi_ds = wvif->scan.n_ssids;
+	scan.ssids = &wvif->scan.ssids[0];
+	scan.scan_req.num_of_channels = it - wvif->scan.curr;
+	scan.scan_req.probe_delay = 100;
+
+	scan.ch = kcalloc(scan.scan_req.num_of_channels, sizeof(u8), GFP_KERNEL);
+
+	if (!scan.ch) {
+		wvif->scan.status = -ENOMEM;
+		goto fail;
+	}
+	for (i = 0; i < scan.scan_req.num_of_channels; ++i)
+		scan.ch[i] = wvif->scan.curr[i]->hw_value;
+
+	if (wvif->scan.curr[0]->flags & IEEE80211_CHAN_NO_IR) {
+		scan.scan_req.min_channel_time = 50;
+		scan.scan_req.max_channel_time = 150;
+	} else {
+		scan.scan_req.min_channel_time = 10;
+		scan.scan_req.max_channel_time = 50;
+	}
+	if (!(first->flags & IEEE80211_CHAN_NO_IR) &&
+	    wvif->scan.output_power != first->max_power) {
+		wvif->scan.output_power = first->max_power;
+		hif_set_output_power(wvif, wvif->scan.output_power * 10);
+	}
+	wvif->scan.status = wfx_scan_start(wvif, &scan);
+	kfree(scan.ch);
+	if (wvif->scan.status)
+		goto fail;
+	wvif->scan.curr = it;
+	mutex_unlock(&wvif->wdev->conf_mutex);
+	return;
+
+fail:
+	wvif->scan.curr = wvif->scan.end;
+	mutex_unlock(&wvif->wdev->conf_mutex);
+	up(&wvif->scan.lock);
+	schedule_work(&wvif->scan.work);
+}
+
+static void wfx_scan_complete(struct wfx_vif *wvif)
+{
+	up(&wvif->scan.lock);
+	atomic_set(&wvif->wdev->scan_in_progress, 0);
+
+	wfx_scan_work(&wvif->scan.work);
+}
+
+void wfx_scan_failed_cb(struct wfx_vif *wvif)
+{
+	if (cancel_delayed_work_sync(&wvif->scan.timeout) > 0) {
+		wvif->scan.status = -EIO;
+		schedule_work(&wvif->scan.timeout.work);
+	}
+}
+
+void wfx_scan_complete_cb(struct wfx_vif *wvif, struct hif_ind_scan_cmpl *arg)
+{
+	if (cancel_delayed_work_sync(&wvif->scan.timeout) > 0) {
+		wvif->scan.status = 1;
+		schedule_work(&wvif->scan.timeout.work);
+	}
+}
+
+void wfx_scan_timeout(struct work_struct *work)
+{
+	struct wfx_vif *wvif = container_of(work, struct wfx_vif, scan.timeout.work);
+
+	if (atomic_xchg(&wvif->scan.in_progress, 0)) {
+		if (wvif->scan.status > 0) {
+			wvif->scan.status = 0;
+		} else if (!wvif->scan.status) {
+			dev_warn(wvif->wdev->dev, "timeout waiting for scan complete notification\n");
+			wvif->scan.status = -ETIMEDOUT;
+			wvif->scan.curr = wvif->scan.end;
+			hif_stop_scan(wvif);
+		}
+		wfx_scan_complete(wvif);
+	}
+}
diff --git a/drivers/staging/wfx/scan.h b/drivers/staging/wfx/scan.h
new file mode 100644
index 000000000000..b4ddd0771a9b
--- /dev/null
+++ b/drivers/staging/wfx/scan.h
@@ -0,0 +1,42 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Scan related functions.
+ *
+ * Copyright (c) 2017-2019, Silicon Laboratories, Inc.
+ * Copyright (c) 2010, ST-Ericsson
+ */
+#ifndef WFX_SCAN_H
+#define WFX_SCAN_H
+
+#include <linux/semaphore.h>
+#include <linux/workqueue.h>
+#include <net/mac80211.h>
+
+#include "hif_api_cmd.h"
+
+struct wfx_dev;
+struct wfx_vif;
+
+struct wfx_scan {
+	struct semaphore lock;
+	struct work_struct work;
+	struct delayed_work timeout;
+	struct cfg80211_scan_request *req;
+	struct ieee80211_channel **begin;
+	struct ieee80211_channel **curr;
+	struct ieee80211_channel **end;
+	struct hif_ssid_def ssids[HIF_API_MAX_NB_SSIDS];
+	int output_power;
+	int n_ssids;
+	int status;
+	atomic_t in_progress;
+};
+
+int wfx_hw_scan(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		struct ieee80211_scan_request *req);
+void wfx_scan_work(struct work_struct *work);
+void wfx_scan_timeout(struct work_struct *work);
+void wfx_scan_complete_cb(struct wfx_vif *wvif, struct hif_ind_scan_cmpl *arg);
+void wfx_scan_failed_cb(struct wfx_vif *wvif);
+
+#endif /* WFX_SCAN_H */
diff --git a/drivers/staging/wfx/sta.c b/drivers/staging/wfx/sta.c
index 5714aba1432c..5a8140100e97 100644
--- a/drivers/staging/wfx/sta.c
+++ b/drivers/staging/wfx/sta.c
@@ -9,9 +9,18 @@
 
 #include "sta.h"
 #include "wfx.h"
+#include "scan.h"
+#include "hif_tx_mib.h"
 
 #define TXOP_UNIT 32
 
+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_tim_impl(struct wfx_vif *wvif, bool aid0_bit_set)
 {
 	struct sk_buff *skb;
@@ -128,6 +137,8 @@ int wfx_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 		default_edca_params[IEEE80211_AC_BK].queue_id = HIF_QUEUE_ID_BESTEFFORT;
 	}
 
+	mutex_lock(&wdev->conf_mutex);
+
 	for (i = 0; i < ARRAY_SIZE(wdev->vif); i++) {
 		if (!wdev->vif[i]) {
 			wdev->vif[i] = vif;
@@ -135,8 +146,10 @@ int wfx_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 			break;
 		}
 	}
-	if (i == ARRAY_SIZE(wdev->vif))
+	if (i == ARRAY_SIZE(wdev->vif)) {
+		mutex_unlock(&wdev->conf_mutex);
 		return -EOPNOTSUPP;
+	}
 	wvif->vif = vif;
 	wvif->wdev = wdev;
 
@@ -148,6 +161,12 @@ int wfx_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 	INIT_WORK(&wvif->mcast_start_work, wfx_mcast_start_work);
 	INIT_WORK(&wvif->mcast_stop_work, wfx_mcast_stop_work);
 	timer_setup(&wvif->mcast_timeout, wfx_mcast_timeout, 0);
+
+	sema_init(&wvif->scan.lock, 1);
+	INIT_WORK(&wvif->scan.work, wfx_scan_work);
+	INIT_DELAYED_WORK(&wvif->scan.timeout, wfx_scan_timeout);
+
+	mutex_unlock(&wdev->conf_mutex);
 	BUG_ON(ARRAY_SIZE(default_edca_params) != ARRAY_SIZE(wvif->edca.params));
 	for (i = 0; i < IEEE80211_NUM_ACS; i++)
 		memcpy(&wvif->edca.params[i], &default_edca_params[i], sizeof(default_edca_params[i]));
@@ -175,7 +194,9 @@ 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");
 }
diff --git a/drivers/staging/wfx/sta.h b/drivers/staging/wfx/sta.h
index f36d94f907c7..dd1b6b3fc2f1 100644
--- a/drivers/staging/wfx/sta.h
+++ b/drivers/staging/wfx/sta.h
@@ -12,6 +12,8 @@
 
 #include "hif_api_cmd.h"
 
+struct wfx_vif;
+
 struct wfx_edca_params {
 	/* NOTE: index is a linux queue id. */
 	struct hif_req_edca_queue_params params[IEEE80211_NUM_ACS];
@@ -29,4 +31,6 @@ void wfx_stop(struct ieee80211_hw *hw);
 int wfx_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif);
 void wfx_remove_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif);
 
+int wfx_fwd_probe_req(struct wfx_vif *wvif, bool enable);
+
 #endif /* WFX_STA_H */
diff --git a/drivers/staging/wfx/wfx.h b/drivers/staging/wfx/wfx.h
index 11775b1e06ef..50c0d9c0e528 100644
--- a/drivers/staging/wfx/wfx.h
+++ b/drivers/staging/wfx/wfx.h
@@ -19,6 +19,7 @@
 #include "queue.h"
 #include "secure_link.h"
 #include "sta.h"
+#include "scan.h"
 #include "hif_tx.h"
 #include "hif_api_general.h"
 
@@ -39,6 +40,7 @@ struct wfx_dev {
 	struct wfx_hif		hif;
 	struct sl_context	sl;
 	int			chip_frozen;
+	struct mutex		conf_mutex;
 
 	struct wfx_hif_cmd	hif_cmd;
 	struct wfx_queue	tx_queue[4];
@@ -48,6 +50,9 @@ struct wfx_dev {
 
 	struct hif_rx_stats	rx_stats;
 	struct mutex		rx_stats_lock;
+
+	int			output_power;
+	atomic_t		scan_in_progress;
 };
 
 struct wfx_vif {
@@ -71,11 +76,17 @@ struct wfx_vif {
 
 	struct tx_policy_cache	tx_policy_cache;
 	struct work_struct	tx_policy_upload_work;
+
 	u32			sta_asleep_mask;
 	u32			pspoll_mask;
 	spinlock_t		ps_state_lock;
 
+	bool			filter_bssid;
+	bool			fwd_probe_req;
+
 	struct wfx_edca_params	edca;
+
+	struct wfx_scan		scan;
 };
 
 static inline struct wfx_vif *wdev_to_wvif(struct wfx_dev *wdev, int vif_id)
-- 
2.20.1