From: Jérôme Pouiller <jerome.pouiller@xxxxxxxxxx>
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@xxxxxxxxxx>
---
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
