[PATCH v3 17/20] staging: wfx: allow to receive 802.11 frames

[Jerome Pouiller <Jerome.Pouiller@silabs.com>]

From: Jérôme Pouiller <jerome.pouiller@silabs.com>

Again, this task is more complex than it should since driver try to
handle itself power saving of stations.

Signed-off-by: Jérôme Pouiller <jerome.pouiller@silabs.com>
---
 drivers/staging/wfx/Makefile  |   1 +
 drivers/staging/wfx/data_rx.c | 182 ++++++++++++++++++++++++++++++++++
 drivers/staging/wfx/data_rx.h |  18 ++++
 drivers/staging/wfx/hif_rx.c  |  23 +++++
 4 files changed, 224 insertions(+)
 create mode 100644 drivers/staging/wfx/data_rx.c
 create mode 100644 drivers/staging/wfx/data_rx.h

diff --git a/drivers/staging/wfx/Makefile b/drivers/staging/wfx/Makefile
index d5ac9fafd1f1..d9e21515d08e 100644
--- a/drivers/staging/wfx/Makefile
+++ b/drivers/staging/wfx/Makefile
@@ -11,6 +11,7 @@ wfx-y := \
 	hif_rx.o \
 	queue.o \
 	data_tx.o \
+	data_rx.o \
 	sta.o \
 	main.o \
 	sta.o \
diff --git a/drivers/staging/wfx/data_rx.c b/drivers/staging/wfx/data_rx.c
new file mode 100644
index 000000000000..3b3117b2edac
--- /dev/null
+++ b/drivers/staging/wfx/data_rx.c
@@ -0,0 +1,182 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Datapath implementation.
+ *
+ * Copyright (c) 2017-2019, Silicon Laboratories, Inc.
+ * Copyright (c) 2010, ST-Ericsson
+ */
+#include <linux/etherdevice.h>
+#include <net/mac80211.h>
+
+#include "data_rx.h"
+#include "wfx.h"
+#include "bh.h"
+#include "sta.h"
+
+static int wfx_handle_pspoll(struct wfx_vif *wvif, struct sk_buff *skb)
+{
+	struct ieee80211_sta *sta;
+	struct ieee80211_pspoll *pspoll = (struct ieee80211_pspoll *)skb->data;
+	int link_id = 0;
+	u32 pspoll_mask = 0;
+	int i;
+
+	if (!ether_addr_equal(wvif->vif->addr, pspoll->bssid))
+		return 1;
+
+	rcu_read_lock();
+	sta = ieee80211_find_sta(wvif->vif, pspoll->ta);
+	if (sta)
+		link_id = ((struct wfx_sta_priv *) &sta->drv_priv)->link_id;
+	rcu_read_unlock();
+	if (link_id)
+		pspoll_mask = BIT(link_id);
+	else
+		return 1;
+
+	wvif->pspoll_mask |= pspoll_mask;
+	/* Do not report pspols if data for given link id is queued already. */
+	for (i = 0; i < IEEE80211_NUM_ACS; ++i) {
+		if (wfx_tx_queue_get_num_queued(&wvif->wdev->tx_queue[i],
+						pspoll_mask)) {
+			wfx_bh_request_tx(wvif->wdev);
+			return 1;
+		}
+	}
+	return 0;
+}
+
+static int wfx_drop_encrypt_data(struct wfx_dev *wdev, struct hif_ind_rx *arg, struct sk_buff *skb)
+{
+	struct ieee80211_hdr *frame = (struct ieee80211_hdr *) skb->data;
+	size_t hdrlen = ieee80211_hdrlen(frame->frame_control);
+	size_t iv_len, icv_len;
+
+	/* 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*/;
+		break;
+	case HIF_RI_FLAGS_AES_ENCRYPTED:
+		iv_len = 8 /* CCMP_HDR_LEN */;
+		icv_len = 8 /* CCMP_MIC_LEN */;
+		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_MICFAILURE)
+		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, struct hif_ind_rx *arg, struct sk_buff *skb)
+{
+	int link_id = arg->rx_flags.peer_sta_id;
+	struct ieee80211_rx_status *hdr = IEEE80211_SKB_RXCB(skb);
+	struct ieee80211_hdr *frame = (struct ieee80211_hdr *) skb->data;
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *) skb->data;
+	struct wfx_link_entry *entry = NULL;
+	bool early_data = false;
+
+	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 (link_id && link_id <= WFX_MAX_STA_IN_AP_MODE) {
+		entry = &wvif->link_id_db[link_id - 1];
+		entry->timestamp = jiffies;
+		if (entry->status == WFX_LINK_SOFT && ieee80211_is_data(frame->frame_control))
+			early_data = true;
+	}
+
+	if (arg->status == HIF_STATUS_MICFAILURE)
+		hdr->flag |= RX_FLAG_MMIC_ERROR;
+	else if (arg->status)
+		goto drop;
+
+	if (skb->len < sizeof(struct ieee80211_pspoll)) {
+		dev_warn(wvif->wdev->dev, "malformed SDU received\n");
+		goto drop;
+	}
+
+	if (ieee80211_is_pspoll(frame->frame_control))
+		if (wfx_handle_pspoll(wvif, skb))
+			goto drop;
+
+	hdr->band = NL80211_BAND_2GHZ;
+	hdr->freq = ieee80211_channel_to_frequency(arg->channel_number, hdr->band);
+
+	if (arg->rxed_rate >= 14) {
+		hdr->encoding = RX_ENC_HT;
+		hdr->rate_idx = arg->rxed_rate - 14;
+	} else if (arg->rxed_rate >= 4) {
+		hdr->rate_idx = arg->rxed_rate - 2;
+	} else {
+		hdr->rate_idx = arg->rxed_rate;
+	}
+
+	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;
+	}
+
+	/* Filter block ACK negotiation: fully controlled by firmware */
+	if (ieee80211_is_action(frame->frame_control)
+	    && arg->rx_flags.match_uc_addr
+	    && mgmt->u.action.category == WLAN_CATEGORY_BACK)
+		goto drop;
+
+	if (early_data) {
+		spin_lock_bh(&wvif->ps_state_lock);
+		/* Double-check status with lock held */
+		if (entry->status == WFX_LINK_SOFT)
+			skb_queue_tail(&entry->rx_queue, skb);
+		else
+			ieee80211_rx_irqsafe(wvif->wdev->hw, skb);
+		spin_unlock_bh(&wvif->ps_state_lock);
+	} else {
+		ieee80211_rx_irqsafe(wvif->wdev->hw, skb);
+	}
+
+	return;
+
+drop:
+	dev_kfree_skb(skb);
+}
diff --git a/drivers/staging/wfx/data_rx.h b/drivers/staging/wfx/data_rx.h
new file mode 100644
index 000000000000..b44d15268f83
--- /dev/null
+++ b/drivers/staging/wfx/data_rx.h
@@ -0,0 +1,18 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Datapath implementation.
+ *
+ * Copyright (c) 2017-2019, Silicon Laboratories, Inc.
+ * Copyright (c) 2010, ST-Ericsson
+ */
+#ifndef WFX_DATA_RX_H
+#define WFX_DATA_RX_H
+
+#include "hif_api_cmd.h"
+
+struct wfx_vif;
+struct sk_buff;
+
+void wfx_rx_cb(struct wfx_vif *wvif, struct hif_ind_rx *arg, struct sk_buff *skb);
+
+#endif /* WFX_DATA_RX_H */
diff --git a/drivers/staging/wfx/hif_rx.c b/drivers/staging/wfx/hif_rx.c
index 97c4c2f082fb..c07984b0535d 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 "data_rx.h"
 #include "secure_link.h"
 #include "hif_api_cmd.h"
 
@@ -127,6 +128,21 @@ static int hif_keys_indication(struct wfx_dev *wdev, struct hif_msg *hif, void *
 	return 0;
 }
 
+static int hif_receive_indication(struct wfx_dev *wdev, struct hif_msg *hif, void *buf, struct sk_buff *skb)
+{
+	struct wfx_vif *wvif = wdev_to_wvif(wdev, hif->interface);
+	struct hif_ind_rx *body = buf;
+
+	if (!wvif) {
+		dev_warn(wdev->dev, "ignore rx data for non existant vif %d\n", hif->interface);
+		return 0;
+	}
+	skb_pull(skb, sizeof(struct hif_msg) + sizeof(struct hif_ind_rx));
+	wfx_rx_cb(wvif, body, skb);
+
+	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);
@@ -218,6 +234,8 @@ static const struct {
 	{ HIF_IND_ID_GENERIC,              hif_generic_indication },
 	{ HIF_IND_ID_ERROR,                hif_error_indication },
 	{ HIF_IND_ID_EXCEPTION,            hif_exception_indication },
+	// FIXME: allocate skb_p from hif_receive_indication and make it generic
+	//{ HIF_IND_ID_RX,                 hif_receive_indication },
 };
 
 void wfx_handle_rx(struct wfx_dev *wdev, struct sk_buff *skb)
@@ -226,6 +244,11 @@ void wfx_handle_rx(struct wfx_dev *wdev, struct sk_buff *skb)
 	struct hif_msg *hif = (struct hif_msg *) skb->data;
 	int hif_id = hif->id;
 
+	if (hif_id == HIF_IND_ID_RX) {
+		// hif_receive_indication take care of skb lifetime
+		hif_receive_indication(wdev, hif, hif->body, skb);
+		return;
+	}
 	// Note: mutex_is_lock cause an implicit memory barrier that protect
 	// buf_send
 	if (mutex_is_locked(&wdev->hif_cmd.lock)
-- 
2.20.1