new file mode 100644
@@ -0,0 +1,257 @@
+// SPDX-License-Identifier: MIT
+/* Copyright(c) 2019-2021, Celeno Communications Ltd. */
+
+#include "rx/rx_amsdu.h"
+
+struct msduhdr {
+ u8 dest[ETH_ALEN];
+ u8 source[ETH_ALEN];
+ __be16 len;
+} __packed;
+
+enum rx_amsdu_error {
+ RX_AMSDU_ERR_CORRUPTED = 0x1,
+ RX_AMSDU_ERR_LENGTH = 0x2,
+};
+
+static void set_flag_amsdu_more(struct sk_buff *skb)
+{
+ struct ieee80211_rx_status *rx_status = IEEE80211_SKB_RXCB(skb);
+
+ rx_status->flag |= RX_FLAG_AMSDU_MORE;
+}
+
+static void clear_flag_amsdu_more(struct sk_buff *skb)
+{
+ struct ieee80211_rx_status *rx_status = IEEE80211_SKB_RXCB(skb);
+
+ rx_status->flag &= ~RX_FLAG_AMSDU_MORE;
+}
+
+static void add_80211_hdr(struct cl_amsdu_rx_state *amsdu_rx_state,
+ struct sk_buff *skb, struct sk_buff *first_skb)
+{
+ /* Copy the 802.11 header of the first skb */
+ struct ieee80211_hdr *hdr_first = (struct ieee80211_hdr *)(first_skb->data);
+ u32 hdrlen_first = ieee80211_hdrlen(hdr_first->frame_control);
+ u32 total_bytes = hdrlen_first + amsdu_rx_state->encrypt_len;
+
+ /* Total_bytes must be smaller than IPC_RXBUF_EXTRA_HEADROOM */
+ skb_push(skb, total_bytes);
+ memcpy(skb->data, first_skb->data, total_bytes);
+}
+
+static void copy_status(struct cl_amsdu_rx_state *amsdu_rx_state,
+ struct sk_buff *skb, struct sk_buff *first_skb)
+{
+ struct ieee80211_rx_status *rx_status = IEEE80211_SKB_RXCB(skb);
+ struct ieee80211_rx_status *rx_status_first = IEEE80211_SKB_RXCB(first_skb);
+
+ /* Copy rx_status from the first skb */
+ memcpy(rx_status, rx_status_first, sizeof(struct ieee80211_rx_status));
+
+ /* If it is the last sub-frame clear RX_FLAG_AMSDU_MORE */
+ if (amsdu_rx_state->msdu_remaining_cnt == 0)
+ rx_status->flag &= ~RX_FLAG_AMSDU_MORE;
+}
+
+static void cl_rx_amsdu_set_state(struct cl_hw *cl_hw, struct sk_buff *skb, struct hw_rxhdr *rxhdr,
+ u8 sta_idx, u8 tid, u32 packet_len, u8 encrypt_len)
+{
+ struct cl_amsdu_rx_state *amsdu_rx_state = &cl_hw->amsdu_rx_state;
+
+ amsdu_rx_state->msdu_cnt = rxhdr->msdu_cnt;
+ amsdu_rx_state->msdu_remaining_cnt = rxhdr->msdu_cnt - 1;
+ amsdu_rx_state->msdu_dma_align = rxhdr->msdu_dma_align;
+ amsdu_rx_state->amsdu_error = 0;
+ amsdu_rx_state->encrypt_len = encrypt_len;
+ amsdu_rx_state->packet_len = packet_len;
+ amsdu_rx_state->rxhdr = rxhdr;
+ amsdu_rx_state->first_skb = skb;
+ amsdu_rx_state->sta_idx = sta_idx;
+ amsdu_rx_state->tid = tid;
+
+ __skb_queue_head(&cl_hw->amsdu_rx_state.frames, skb);
+}
+
+static void cl_rx_amsdu_set_state_error(struct cl_hw *cl_hw,
+ struct hw_rxhdr *rxhdr,
+ enum rx_amsdu_error err)
+{
+ struct cl_amsdu_rx_state *amsdu_rx_state = &cl_hw->amsdu_rx_state;
+
+ amsdu_rx_state->msdu_cnt = rxhdr->msdu_cnt;
+ amsdu_rx_state->msdu_remaining_cnt = rxhdr->msdu_cnt - 1;
+ amsdu_rx_state->amsdu_error = err;
+}
+
+static void cl_rx_amsdu_first_length_error(struct cl_hw *cl_hw, struct sk_buff *skb,
+ struct hw_rxhdr *rxhdr, u32 len)
+{
+ cl_dbg_err(cl_hw, "RX-AMSDU length error (1/%u) - tailroom=%d, len=%u\n",
+ rxhdr->msdu_cnt, skb_tailroom(skb), len);
+
+ cl_rx_amsdu_set_state_error(cl_hw, rxhdr, RX_AMSDU_ERR_LENGTH);
+
+ cl_hw->rx_info.pkt_drop_amsdu_len_error++;
+ kfree_skb(skb);
+}
+
+static void cl_rx_amsdu_sub_length_error(struct cl_hw *cl_hw, struct sk_buff *skb, u32 len)
+{
+ struct cl_amsdu_rx_state *amsdu_rx_state = &cl_hw->amsdu_rx_state;
+ struct sk_buff *skb_tail;
+ u8 sub_cnt = amsdu_rx_state->msdu_cnt - amsdu_rx_state->msdu_remaining_cnt;
+
+ cl_dbg_err(cl_hw, "RX-AMSDU length error (%u/%u) - tailroom=%d, len=%u\n",
+ sub_cnt, amsdu_rx_state->msdu_cnt, skb_tailroom(skb), len);
+
+ /* All remaining skbs in the AMSDU will be treated as errors */
+ amsdu_rx_state->amsdu_error = RX_AMSDU_ERR_LENGTH;
+
+ /* Clear RX_FLAG_AMSDU_MORE in the last success skb that was received */
+ skb_tail = skb_peek_tail(&amsdu_rx_state->frames);
+ clear_flag_amsdu_more(skb_tail);
+
+ cl_hw->rx_info.pkt_drop_sub_amsdu_len_error++;
+ kfree_skb(skb);
+}
+
+void cl_rx_amsdu_first(struct cl_hw *cl_hw, struct sk_buff *skb,
+ struct hw_rxhdr *rxhdr, u8 sta_idx, u8 tid, u8 encrypt_len)
+{
+ struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)(skb->data);
+ u32 hdr_len = ieee80211_hdrlen(hdr->frame_control);
+ struct msduhdr *msdu_hdr = (struct msduhdr *)(skb->data + hdr_len + encrypt_len);
+ u32 packet_len = hdr_len + encrypt_len + sizeof(struct msduhdr) + ntohs(msdu_hdr->len);
+
+ if (skb_tailroom(skb) < packet_len) {
+ cl_rx_amsdu_first_length_error(cl_hw, skb, rxhdr, packet_len);
+ return;
+ }
+
+ /* Put the WLAN header + MSDU header + payload in the skb data */
+ skb_put(skb, packet_len);
+
+ cl_rx_amsdu_set_state(cl_hw, skb, rxhdr, sta_idx, tid, packet_len, encrypt_len);
+
+ /* Must be called after cl_rx_amsdu_set_state() */
+ if (cl_hw->amsdu_rx_state.msdu_remaining_cnt > 0)
+ set_flag_amsdu_more(skb);
+}
+
+bool cl_rx_amsdu_sub(struct cl_hw *cl_hw, struct sk_buff *skb)
+{
+ /*
+ * ----------------------------------------------------------
+ * | DMA padding 4 byte alignment | MSDU HDR | MSDU PAYLOAD |
+ * ---------------------------------------------------------
+ */
+ struct cl_amsdu_rx_state *amsdu_rx_state = &cl_hw->amsdu_rx_state;
+ struct sk_buff *first_skb = amsdu_rx_state->first_skb;
+ struct msduhdr *msdu_hdr;
+ u32 packet_len;
+
+ /*
+ * Push the dma alignment to the reserved area, so that skb->data will
+ * point to the MSDU header
+ */
+ skb_reserve(skb, amsdu_rx_state->msdu_dma_align);
+
+ msdu_hdr = (struct msduhdr *)(skb->data);
+ packet_len = sizeof(struct msduhdr) + ntohs(msdu_hdr->len);
+
+ if (skb_tailroom(skb) < packet_len) {
+ cl_rx_amsdu_sub_length_error(cl_hw, skb, packet_len);
+ return false;
+ }
+
+ /* Put the MSDU HDR + MSDU PAYLOAD into the skb data area */
+ skb_put(skb, packet_len);
+
+ amsdu_rx_state->packet_len += packet_len;
+
+ add_80211_hdr(amsdu_rx_state, skb, first_skb);
+ copy_status(amsdu_rx_state, skb, first_skb);
+
+ /* Store the pointer to sta in the skb->sk field */
+ skb->sk = first_skb->sk;
+
+ __skb_queue_tail(&amsdu_rx_state->frames, skb);
+
+ return true;
+}
+
+void cl_rx_amsdu_first_corrupted(struct cl_hw *cl_hw, struct sk_buff *skb,
+ struct hw_rxhdr *rxhdr)
+{
+ struct ieee80211_hdr *mac_hdr = (struct ieee80211_hdr *)(skb->data);
+
+ cl_dbg_verbose(cl_hw, "Corrupted RX-AMSDU (1/%u), dest_addr=%pM\n",
+ rxhdr->msdu_cnt, mac_hdr->addr1);
+
+ cl_rx_amsdu_set_state_error(cl_hw, rxhdr, RX_AMSDU_ERR_CORRUPTED);
+
+ cl_hw->rx_info.pkt_drop_amsdu_corrupted++;
+ kfree_skb(skb);
+}
+
+void cl_rx_amsdu_sub_error(struct cl_hw *cl_hw, struct sk_buff *skb)
+{
+ struct cl_amsdu_rx_state *amsdu_rx_state = &cl_hw->amsdu_rx_state;
+ u8 sub_cnt = amsdu_rx_state->msdu_cnt - amsdu_rx_state->msdu_remaining_cnt;
+
+ if (amsdu_rx_state->amsdu_error & RX_AMSDU_ERR_CORRUPTED) {
+ cl_hw->rx_info.pkt_drop_sub_amsdu_corrupted++;
+
+ cl_dbg_verbose(cl_hw, "Corrupted RX-AMSDU (%u/%u)\n",
+ sub_cnt, amsdu_rx_state->msdu_cnt);
+ } else if (amsdu_rx_state->amsdu_error & RX_AMSDU_ERR_LENGTH) {
+ cl_hw->rx_info.pkt_drop_sub_amsdu_len_error++;
+
+ cl_dbg_verbose(cl_hw, "RX-AMSDU length error (%u/%u)\n",
+ sub_cnt, amsdu_rx_state->msdu_cnt);
+ }
+
+ kfree_skb(skb);
+}
+
+void cl_rx_amsdu_reset(struct cl_hw *cl_hw)
+{
+ /* Free pending frames */
+ __skb_queue_purge(&cl_hw->amsdu_rx_state.frames);
+
+ /* Reset RX A-MSDU state */
+ memset(&cl_hw->amsdu_rx_state, 0, sizeof(struct cl_amsdu_rx_state));
+
+ __skb_queue_head_init(&cl_hw->amsdu_rx_state.frames);
+}
+
+void cl_rx_amsdu_stats(struct cl_hw *cl_hw, u8 msdu_cnt)
+{
+ /*
+ * Update A-MSDU statistics
+ * msdu_cnt 1 - 128 is mapped to 0 - 127.
+ */
+ if (msdu_cnt <= RX_MAX_MSDU_IN_AMSDU)
+ cl_hw->rx_info.amsdu_cnt[msdu_cnt - 1]++;
+ else
+ cl_dbg_err(cl_hw, "Invalid msdu_cnt [%u]\n", msdu_cnt);
+}
+
+/* Only ieee80211_hw_set() is defined in mac80211.h */
+static inline void _ieee80211_hw_clear(struct ieee80211_hw *hw,
+ enum ieee80211_hw_flags flg)
+{
+ return __clear_bit(flg, hw->flags);
+}
+
+#define ieee80211_hw_clear(hw, flg) _ieee80211_hw_clear(hw, IEEE80211_HW_##flg)
+
+void cl_rx_amsdu_hw_en(struct ieee80211_hw *hw, bool rxamsdu_en)
+{
+ if (rxamsdu_en)
+ ieee80211_hw_set(hw, SUPPORTS_AMSDU_IN_AMPDU);
+ else
+ ieee80211_hw_clear(hw, SUPPORTS_AMSDU_IN_AMPDU);
+}