+++ /dev/null
-From 986e43b19ae9176093da35e0a844e65c8bf9ede7 Mon Sep 17 00:00:00 2001
-From: Felix Fietkau <nbd@nbd.name>
-Date: Mon, 13 Feb 2023 11:08:54 +0100
-Subject: [PATCH] wifi: mac80211: fix receiving A-MSDU frames on mesh
- interfaces
-
-The current mac80211 mesh A-MSDU receive path fails to parse A-MSDU packets
-on mesh interfaces, because it assumes that the Mesh Control field is always
-directly after the 802.11 header.
-802.11-2020 9.3.2.2.2 Figure 9-70 shows that the Mesh Control field is
-actually part of the A-MSDU subframe header.
-This makes more sense, since it allows packets for multiple different
-destinations to be included in the same A-MSDU, as long as RA and TID are
-still the same.
-Another issue is the fact that the A-MSDU subframe length field was apparently
-accidentally defined as little-endian in the standard.
-
-In order to fix this, the mesh forwarding path needs happen at a different
-point in the receive path.
-
-ieee80211_data_to_8023_exthdr is changed to ignore the mesh control field
-and leave it in after the ethernet header. This also affects the source/dest
-MAC address fields, which now in the case of mesh point to the mesh SA/DA.
-
-ieee80211_amsdu_to_8023s is changed to deal with the endian difference and
-to add the Mesh Control length to the subframe length, since it's not covered
-by the MSDU length field.
-
-With these changes, the mac80211 will get the same packet structure for
-converted regular data packets and unpacked A-MSDU subframes.
-
-The mesh forwarding checks are now only performed after the A-MSDU decap.
-For locally received packets, the Mesh Control header is stripped away.
-For forwarded packets, a new 802.11 header gets added.
-
-Signed-off-by: Felix Fietkau <nbd@nbd.name>
-Link: https://lore.kernel.org/r/20230213100855.34315-4-nbd@nbd.name
-[fix fortify build error]
-Signed-off-by: Johannes Berg <johannes.berg@intel.com>
----
- .../wireless/marvell/mwifiex/11n_rxreorder.c | 2 +-
- include/net/cfg80211.h | 27 +-
- net/mac80211/rx.c | 350 ++++++++++--------
- net/wireless/util.c | 120 +++---
- 4 files changed, 297 insertions(+), 202 deletions(-)
-
---- a/drivers/net/wireless/marvell/mwifiex/11n_rxreorder.c
-+++ b/drivers/net/wireless/marvell/mwifiex/11n_rxreorder.c
-@@ -33,7 +33,7 @@ static int mwifiex_11n_dispatch_amsdu_pk
- skb_trim(skb, le16_to_cpu(local_rx_pd->rx_pkt_length));
-
- ieee80211_amsdu_to_8023s(skb, &list, priv->curr_addr,
-- priv->wdev.iftype, 0, NULL, NULL);
-+ priv->wdev.iftype, 0, NULL, NULL, false);
-
- while (!skb_queue_empty(&list)) {
- struct rx_packet_hdr *rx_hdr;
---- a/include/net/cfg80211.h
-+++ b/include/net/cfg80211.h
-@@ -6208,11 +6208,36 @@ static inline int ieee80211_data_to_8023
- * @extra_headroom: The hardware extra headroom for SKBs in the @list.
- * @check_da: DA to check in the inner ethernet header, or NULL
- * @check_sa: SA to check in the inner ethernet header, or NULL
-+ * @mesh_control: A-MSDU subframe header includes the mesh control field
- */
- void ieee80211_amsdu_to_8023s(struct sk_buff *skb, struct sk_buff_head *list,
- const u8 *addr, enum nl80211_iftype iftype,
- const unsigned int extra_headroom,
-- const u8 *check_da, const u8 *check_sa);
-+ const u8 *check_da, const u8 *check_sa,
-+ bool mesh_control);
-+
-+/**
-+ * ieee80211_get_8023_tunnel_proto - get RFC1042 or bridge tunnel encap protocol
-+ *
-+ * Check for RFC1042 or bridge tunnel header and fetch the encapsulated
-+ * protocol.
-+ *
-+ * @hdr: pointer to the MSDU payload
-+ * @proto: destination pointer to store the protocol
-+ * Return: true if encapsulation was found
-+ */
-+bool ieee80211_get_8023_tunnel_proto(const void *hdr, __be16 *proto);
-+
-+/**
-+ * ieee80211_strip_8023_mesh_hdr - strip mesh header from converted 802.3 frames
-+ *
-+ * Strip the mesh header, which was left in by ieee80211_data_to_8023 as part
-+ * of the MSDU data. Also move any source/destination addresses from the mesh
-+ * header to the ethernet header (if present).
-+ *
-+ * @skb: The 802.3 frame with embedded mesh header
-+ */
-+int ieee80211_strip_8023_mesh_hdr(struct sk_buff *skb);
-
- /**
- * cfg80211_classify8021d - determine the 802.1p/1d tag for a data frame
---- a/net/mac80211/rx.c
-+++ b/net/mac80211/rx.c
-@@ -2720,6 +2720,174 @@ ieee80211_deliver_skb(struct ieee80211_r
- }
- }
-
-+static ieee80211_rx_result
-+ieee80211_rx_mesh_data(struct ieee80211_sub_if_data *sdata, struct sta_info *sta,
-+ struct sk_buff *skb)
-+{
-+#ifdef CPTCFG_MAC80211_MESH
-+ struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
-+ struct ieee80211_local *local = sdata->local;
-+ uint16_t fc = IEEE80211_FTYPE_DATA | IEEE80211_STYPE_QOS_DATA;
-+ struct ieee80211_hdr hdr = {
-+ .frame_control = cpu_to_le16(fc)
-+ };
-+ struct ieee80211_hdr *fwd_hdr;
-+ struct ieee80211s_hdr *mesh_hdr;
-+ struct ieee80211_tx_info *info;
-+ struct sk_buff *fwd_skb;
-+ struct ethhdr *eth;
-+ bool multicast;
-+ int tailroom = 0;
-+ int hdrlen, mesh_hdrlen;
-+ u8 *qos;
-+
-+ if (!ieee80211_vif_is_mesh(&sdata->vif))
-+ return RX_CONTINUE;
-+
-+ if (!pskb_may_pull(skb, sizeof(*eth) + 6))
-+ return RX_DROP_MONITOR;
-+
-+ mesh_hdr = (struct ieee80211s_hdr *)(skb->data + sizeof(*eth));
-+ mesh_hdrlen = ieee80211_get_mesh_hdrlen(mesh_hdr);
-+
-+ if (!pskb_may_pull(skb, sizeof(*eth) + mesh_hdrlen))
-+ return RX_DROP_MONITOR;
-+
-+ eth = (struct ethhdr *)skb->data;
-+ multicast = is_multicast_ether_addr(eth->h_dest);
-+
-+ mesh_hdr = (struct ieee80211s_hdr *)(eth + 1);
-+ if (!mesh_hdr->ttl)
-+ return RX_DROP_MONITOR;
-+
-+ /* frame is in RMC, don't forward */
-+ if (is_multicast_ether_addr(eth->h_dest) &&
-+ mesh_rmc_check(sdata, eth->h_source, mesh_hdr))
-+ return RX_DROP_MONITOR;
-+
-+ /* Frame has reached destination. Don't forward */
-+ if (ether_addr_equal(sdata->vif.addr, eth->h_dest))
-+ goto rx_accept;
-+
-+ if (!ifmsh->mshcfg.dot11MeshForwarding) {
-+ if (is_multicast_ether_addr(eth->h_dest))
-+ goto rx_accept;
-+
-+ return RX_DROP_MONITOR;
-+ }
-+
-+ /* forward packet */
-+ if (sdata->crypto_tx_tailroom_needed_cnt)
-+ tailroom = IEEE80211_ENCRYPT_TAILROOM;
-+
-+ if (!--mesh_hdr->ttl) {
-+ if (multicast)
-+ goto rx_accept;
-+
-+ IEEE80211_IFSTA_MESH_CTR_INC(ifmsh, dropped_frames_ttl);
-+ return RX_DROP_MONITOR;
-+ }
-+
-+ if (mesh_hdr->flags & MESH_FLAGS_AE) {
-+ struct mesh_path *mppath;
-+ char *proxied_addr;
-+
-+ if (multicast)
-+ proxied_addr = mesh_hdr->eaddr1;
-+ else if ((mesh_hdr->flags & MESH_FLAGS_AE) == MESH_FLAGS_AE_A5_A6)
-+ /* has_a4 already checked in ieee80211_rx_mesh_check */
-+ proxied_addr = mesh_hdr->eaddr2;
-+ else
-+ return RX_DROP_MONITOR;
-+
-+ rcu_read_lock();
-+ mppath = mpp_path_lookup(sdata, proxied_addr);
-+ if (!mppath) {
-+ mpp_path_add(sdata, proxied_addr, eth->h_source);
-+ } else {
-+ spin_lock_bh(&mppath->state_lock);
-+ if (!ether_addr_equal(mppath->mpp, eth->h_source))
-+ memcpy(mppath->mpp, eth->h_source, ETH_ALEN);
-+ mppath->exp_time = jiffies;
-+ spin_unlock_bh(&mppath->state_lock);
-+ }
-+ rcu_read_unlock();
-+ }
-+
-+ skb_set_queue_mapping(skb, ieee802_1d_to_ac[skb->priority]);
-+
-+ ieee80211_fill_mesh_addresses(&hdr, &hdr.frame_control,
-+ eth->h_dest, eth->h_source);
-+ hdrlen = ieee80211_hdrlen(hdr.frame_control);
-+ if (multicast) {
-+ int extra_head = sizeof(struct ieee80211_hdr) - sizeof(*eth);
-+
-+ fwd_skb = skb_copy_expand(skb, local->tx_headroom + extra_head +
-+ IEEE80211_ENCRYPT_HEADROOM,
-+ tailroom, GFP_ATOMIC);
-+ if (!fwd_skb)
-+ goto rx_accept;
-+ } else {
-+ fwd_skb = skb;
-+ skb = NULL;
-+
-+ if (skb_cow_head(fwd_skb, hdrlen - sizeof(struct ethhdr)))
-+ return RX_DROP_UNUSABLE;
-+ }
-+
-+ fwd_hdr = skb_push(fwd_skb, hdrlen - sizeof(struct ethhdr));
-+ memcpy(fwd_hdr, &hdr, hdrlen - 2);
-+ qos = ieee80211_get_qos_ctl(fwd_hdr);
-+ qos[0] = qos[1] = 0;
-+
-+ skb_reset_mac_header(fwd_skb);
-+ hdrlen += mesh_hdrlen;
-+ if (ieee80211_get_8023_tunnel_proto(fwd_skb->data + hdrlen,
-+ &fwd_skb->protocol))
-+ hdrlen += ETH_ALEN;
-+ else
-+ fwd_skb->protocol = htons(fwd_skb->len - hdrlen);
-+ skb_set_network_header(fwd_skb, hdrlen);
-+
-+ info = IEEE80211_SKB_CB(fwd_skb);
-+ memset(info, 0, sizeof(*info));
-+ info->control.flags |= IEEE80211_TX_INTCFL_NEED_TXPROCESSING;
-+ info->control.vif = &sdata->vif;
-+ info->control.jiffies = jiffies;
-+ if (multicast) {
-+ IEEE80211_IFSTA_MESH_CTR_INC(ifmsh, fwded_mcast);
-+ memcpy(fwd_hdr->addr2, sdata->vif.addr, ETH_ALEN);
-+ /* update power mode indication when forwarding */
-+ ieee80211_mps_set_frame_flags(sdata, NULL, fwd_hdr);
-+ } else if (!mesh_nexthop_lookup(sdata, fwd_skb)) {
-+ /* mesh power mode flags updated in mesh_nexthop_lookup */
-+ IEEE80211_IFSTA_MESH_CTR_INC(ifmsh, fwded_unicast);
-+ } else {
-+ /* unable to resolve next hop */
-+ if (sta)
-+ mesh_path_error_tx(sdata, ifmsh->mshcfg.element_ttl,
-+ hdr.addr3, 0,
-+ WLAN_REASON_MESH_PATH_NOFORWARD,
-+ sta->sta.addr);
-+ IEEE80211_IFSTA_MESH_CTR_INC(ifmsh, dropped_frames_no_route);
-+ kfree_skb(fwd_skb);
-+ goto rx_accept;
-+ }
-+
-+ IEEE80211_IFSTA_MESH_CTR_INC(ifmsh, fwded_frames);
-+ fwd_skb->dev = sdata->dev;
-+ ieee80211_add_pending_skb(local, fwd_skb);
-+
-+rx_accept:
-+ if (!skb)
-+ return RX_QUEUED;
-+
-+ ieee80211_strip_8023_mesh_hdr(skb);
-+#endif
-+
-+ return RX_CONTINUE;
-+}
-+
- static ieee80211_rx_result debug_noinline
- __ieee80211_rx_h_amsdu(struct ieee80211_rx_data *rx, u8 data_offset)
- {
-@@ -2728,8 +2896,10 @@ __ieee80211_rx_h_amsdu(struct ieee80211_
- struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
- __le16 fc = hdr->frame_control;
- struct sk_buff_head frame_list;
-+ static ieee80211_rx_result res;
- struct ethhdr ethhdr;
- const u8 *check_da = ethhdr.h_dest, *check_sa = ethhdr.h_source;
-+ bool mesh = false;
-
- if (unlikely(ieee80211_has_a4(hdr->frame_control))) {
- check_da = NULL;
-@@ -2746,6 +2916,8 @@ __ieee80211_rx_h_amsdu(struct ieee80211_
- break;
- case NL80211_IFTYPE_MESH_POINT:
- check_sa = NULL;
-+ check_da = NULL;
-+ mesh = true;
- break;
- default:
- break;
-@@ -2763,17 +2935,29 @@ __ieee80211_rx_h_amsdu(struct ieee80211_
- ieee80211_amsdu_to_8023s(skb, &frame_list, dev->dev_addr,
- rx->sdata->vif.type,
- rx->local->hw.extra_tx_headroom,
-- check_da, check_sa);
-+ check_da, check_sa, mesh);
-
- while (!skb_queue_empty(&frame_list)) {
- rx->skb = __skb_dequeue(&frame_list);
-
-- if (!ieee80211_frame_allowed(rx, fc)) {
-- dev_kfree_skb(rx->skb);
-+ res = ieee80211_rx_mesh_data(rx->sdata, rx->sta, rx->skb);
-+ switch (res) {
-+ case RX_QUEUED:
- continue;
-+ case RX_CONTINUE:
-+ break;
-+ default:
-+ goto free;
- }
-
-+ if (!ieee80211_frame_allowed(rx, fc))
-+ goto free;
-+
- ieee80211_deliver_skb(rx);
-+ continue;
-+
-+free:
-+ dev_kfree_skb(rx->skb);
- }
-
- return RX_QUEUED;
-@@ -2806,6 +2990,8 @@ ieee80211_rx_h_amsdu(struct ieee80211_rx
- if (!rx->sdata->u.mgd.use_4addr)
- return RX_DROP_UNUSABLE;
- break;
-+ case NL80211_IFTYPE_MESH_POINT:
-+ break;
- default:
- return RX_DROP_UNUSABLE;
- }
-@@ -2834,155 +3020,6 @@ ieee80211_rx_h_amsdu(struct ieee80211_rx
- return __ieee80211_rx_h_amsdu(rx, 0);
- }
-
--#ifdef CPTCFG_MAC80211_MESH
--static ieee80211_rx_result
--ieee80211_rx_h_mesh_fwding(struct ieee80211_rx_data *rx)
--{
-- struct ieee80211_hdr *fwd_hdr, *hdr;
-- struct ieee80211_tx_info *info;
-- struct ieee80211s_hdr *mesh_hdr;
-- struct sk_buff *skb = rx->skb, *fwd_skb;
-- struct ieee80211_local *local = rx->local;
-- struct ieee80211_sub_if_data *sdata = rx->sdata;
-- struct ieee80211_if_mesh *ifmsh = &sdata->u.mesh;
-- u16 ac, q, hdrlen;
-- int tailroom = 0;
--
-- hdr = (struct ieee80211_hdr *) skb->data;
-- hdrlen = ieee80211_hdrlen(hdr->frame_control);
--
-- /* make sure fixed part of mesh header is there, also checks skb len */
-- if (!pskb_may_pull(rx->skb, hdrlen + 6))
-- return RX_DROP_MONITOR;
--
-- mesh_hdr = (struct ieee80211s_hdr *) (skb->data + hdrlen);
--
-- /* make sure full mesh header is there, also checks skb len */
-- if (!pskb_may_pull(rx->skb,
-- hdrlen + ieee80211_get_mesh_hdrlen(mesh_hdr)))
-- return RX_DROP_MONITOR;
--
-- /* reload pointers */
-- hdr = (struct ieee80211_hdr *) skb->data;
-- mesh_hdr = (struct ieee80211s_hdr *) (skb->data + hdrlen);
--
-- if (ieee80211_drop_unencrypted(rx, hdr->frame_control)) {
-- int offset = hdrlen + ieee80211_get_mesh_hdrlen(mesh_hdr) +
-- sizeof(rfc1042_header);
-- __be16 ethertype;
--
-- if (!ether_addr_equal(hdr->addr1, rx->sdata->vif.addr) ||
-- skb_copy_bits(rx->skb, offset, ðertype, 2) != 0 ||
-- ethertype != rx->sdata->control_port_protocol)
-- return RX_DROP_MONITOR;
-- }
--
-- /* frame is in RMC, don't forward */
-- if (ieee80211_is_data(hdr->frame_control) &&
-- is_multicast_ether_addr(hdr->addr1) &&
-- mesh_rmc_check(rx->sdata, hdr->addr3, mesh_hdr))
-- return RX_DROP_MONITOR;
--
-- if (!ieee80211_is_data(hdr->frame_control))
-- return RX_CONTINUE;
--
-- if (!mesh_hdr->ttl)
-- return RX_DROP_MONITOR;
--
-- if (mesh_hdr->flags & MESH_FLAGS_AE) {
-- struct mesh_path *mppath;
-- char *proxied_addr;
-- char *mpp_addr;
--
-- if (is_multicast_ether_addr(hdr->addr1)) {
-- mpp_addr = hdr->addr3;
-- proxied_addr = mesh_hdr->eaddr1;
-- } else if ((mesh_hdr->flags & MESH_FLAGS_AE) ==
-- MESH_FLAGS_AE_A5_A6) {
-- /* has_a4 already checked in ieee80211_rx_mesh_check */
-- mpp_addr = hdr->addr4;
-- proxied_addr = mesh_hdr->eaddr2;
-- } else {
-- return RX_DROP_MONITOR;
-- }
--
-- rcu_read_lock();
-- mppath = mpp_path_lookup(sdata, proxied_addr);
-- if (!mppath) {
-- mpp_path_add(sdata, proxied_addr, mpp_addr);
-- } else {
-- spin_lock_bh(&mppath->state_lock);
-- if (!ether_addr_equal(mppath->mpp, mpp_addr))
-- memcpy(mppath->mpp, mpp_addr, ETH_ALEN);
-- mppath->exp_time = jiffies;
-- spin_unlock_bh(&mppath->state_lock);
-- }
-- rcu_read_unlock();
-- }
--
-- /* Frame has reached destination. Don't forward */
-- if (!is_multicast_ether_addr(hdr->addr1) &&
-- ether_addr_equal(sdata->vif.addr, hdr->addr3))
-- return RX_CONTINUE;
--
-- ac = ieee802_1d_to_ac[skb->priority];
-- skb_set_queue_mapping(skb, ac);
--
-- if (!--mesh_hdr->ttl) {
-- if (!is_multicast_ether_addr(hdr->addr1))
-- IEEE80211_IFSTA_MESH_CTR_INC(ifmsh,
-- dropped_frames_ttl);
-- goto out;
-- }
--
-- if (!ifmsh->mshcfg.dot11MeshForwarding)
-- goto out;
--
-- if (sdata->crypto_tx_tailroom_needed_cnt)
-- tailroom = IEEE80211_ENCRYPT_TAILROOM;
--
-- fwd_skb = skb_copy_expand(skb, local->tx_headroom +
-- IEEE80211_ENCRYPT_HEADROOM,
-- tailroom, GFP_ATOMIC);
-- if (!fwd_skb)
-- goto out;
--
-- fwd_skb->dev = sdata->dev;
-- fwd_hdr = (struct ieee80211_hdr *) fwd_skb->data;
-- fwd_hdr->frame_control &= ~cpu_to_le16(IEEE80211_FCTL_RETRY);
-- info = IEEE80211_SKB_CB(fwd_skb);
-- memset(info, 0, sizeof(*info));
-- info->control.flags |= IEEE80211_TX_INTCFL_NEED_TXPROCESSING;
-- info->control.vif = &rx->sdata->vif;
-- info->control.jiffies = jiffies;
-- if (is_multicast_ether_addr(fwd_hdr->addr1)) {
-- IEEE80211_IFSTA_MESH_CTR_INC(ifmsh, fwded_mcast);
-- memcpy(fwd_hdr->addr2, sdata->vif.addr, ETH_ALEN);
-- /* update power mode indication when forwarding */
-- ieee80211_mps_set_frame_flags(sdata, NULL, fwd_hdr);
-- } else if (!mesh_nexthop_lookup(sdata, fwd_skb)) {
-- /* mesh power mode flags updated in mesh_nexthop_lookup */
-- IEEE80211_IFSTA_MESH_CTR_INC(ifmsh, fwded_unicast);
-- } else {
-- /* unable to resolve next hop */
-- mesh_path_error_tx(sdata, ifmsh->mshcfg.element_ttl,
-- fwd_hdr->addr3, 0,
-- WLAN_REASON_MESH_PATH_NOFORWARD,
-- fwd_hdr->addr2);
-- IEEE80211_IFSTA_MESH_CTR_INC(ifmsh, dropped_frames_no_route);
-- kfree_skb(fwd_skb);
-- return RX_DROP_MONITOR;
-- }
--
-- IEEE80211_IFSTA_MESH_CTR_INC(ifmsh, fwded_frames);
-- ieee80211_add_pending_skb(local, fwd_skb);
-- out:
-- if (is_multicast_ether_addr(hdr->addr1))
-- return RX_CONTINUE;
-- return RX_DROP_MONITOR;
--}
--#endif
--
- static ieee80211_rx_result debug_noinline
- ieee80211_rx_h_data(struct ieee80211_rx_data *rx)
- {
-@@ -2991,6 +3028,7 @@ ieee80211_rx_h_data(struct ieee80211_rx_
- struct net_device *dev = sdata->dev;
- struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)rx->skb->data;
- __le16 fc = hdr->frame_control;
-+ static ieee80211_rx_result res;
- bool port_control;
- int err;
-
-@@ -3017,6 +3055,10 @@ ieee80211_rx_h_data(struct ieee80211_rx_
- if (unlikely(err))
- return RX_DROP_UNUSABLE;
-
-+ res = ieee80211_rx_mesh_data(rx->sdata, rx->sta, rx->skb);
-+ if (res != RX_CONTINUE)
-+ return res;
-+
- if (!ieee80211_frame_allowed(rx, fc))
- return RX_DROP_MONITOR;
-
-@@ -3987,10 +4029,6 @@ static void ieee80211_rx_handlers(struct
- CALL_RXH(ieee80211_rx_h_defragment);
- CALL_RXH(ieee80211_rx_h_michael_mic_verify);
- /* must be after MMIC verify so header is counted in MPDU mic */
--#ifdef CPTCFG_MAC80211_MESH
-- if (ieee80211_vif_is_mesh(&rx->sdata->vif))
-- CALL_RXH(ieee80211_rx_h_mesh_fwding);
--#endif
- CALL_RXH(ieee80211_rx_h_amsdu);
- CALL_RXH(ieee80211_rx_h_data);
-
---- a/net/wireless/util.c
-+++ b/net/wireless/util.c
-@@ -542,7 +542,7 @@ unsigned int ieee80211_get_mesh_hdrlen(s
- }
- EXPORT_SYMBOL(ieee80211_get_mesh_hdrlen);
-
--static bool ieee80211_get_8023_tunnel_proto(const void *hdr, __be16 *proto)
-+bool ieee80211_get_8023_tunnel_proto(const void *hdr, __be16 *proto)
- {
- const __be16 *hdr_proto = hdr + ETH_ALEN;
-
-@@ -556,6 +556,49 @@ static bool ieee80211_get_8023_tunnel_pr
-
- return true;
- }
-+EXPORT_SYMBOL(ieee80211_get_8023_tunnel_proto);
-+
-+int ieee80211_strip_8023_mesh_hdr(struct sk_buff *skb)
-+{
-+ const void *mesh_addr;
-+ struct {
-+ struct ethhdr eth;
-+ u8 flags;
-+ } payload;
-+ int hdrlen;
-+ int ret;
-+
-+ ret = skb_copy_bits(skb, 0, &payload, sizeof(payload));
-+ if (ret)
-+ return ret;
-+
-+ hdrlen = sizeof(payload.eth) + __ieee80211_get_mesh_hdrlen(payload.flags);
-+
-+ if (likely(pskb_may_pull(skb, hdrlen + 8) &&
-+ ieee80211_get_8023_tunnel_proto(skb->data + hdrlen,
-+ &payload.eth.h_proto)))
-+ hdrlen += ETH_ALEN + 2;
-+ else if (!pskb_may_pull(skb, hdrlen))
-+ return -EINVAL;
-+
-+ mesh_addr = skb->data + sizeof(payload.eth) + ETH_ALEN;
-+ switch (payload.flags & MESH_FLAGS_AE) {
-+ case MESH_FLAGS_AE_A4:
-+ memcpy(&payload.eth.h_source, mesh_addr, ETH_ALEN);
-+ break;
-+ case MESH_FLAGS_AE_A5_A6:
-+ memcpy(&payload.eth, mesh_addr, 2 * ETH_ALEN);
-+ break;
-+ default:
-+ break;
-+ }
-+
-+ pskb_pull(skb, hdrlen - sizeof(payload.eth));
-+ memcpy(skb->data, &payload.eth, sizeof(payload.eth));
-+
-+ return 0;
-+}
-+EXPORT_SYMBOL(ieee80211_strip_8023_mesh_hdr);
-
- int ieee80211_data_to_8023_exthdr(struct sk_buff *skb, struct ethhdr *ehdr,
- const u8 *addr, enum nl80211_iftype iftype,
-@@ -568,7 +611,6 @@ int ieee80211_data_to_8023_exthdr(struct
- } payload;
- struct ethhdr tmp;
- u16 hdrlen;
-- u8 mesh_flags = 0;
-
- if (unlikely(!ieee80211_is_data_present(hdr->frame_control)))
- return -1;
-@@ -589,12 +631,6 @@ int ieee80211_data_to_8023_exthdr(struct
- memcpy(tmp.h_dest, ieee80211_get_DA(hdr), ETH_ALEN);
- memcpy(tmp.h_source, ieee80211_get_SA(hdr), ETH_ALEN);
-
-- if (iftype == NL80211_IFTYPE_MESH_POINT &&
-- skb_copy_bits(skb, hdrlen, &mesh_flags, 1) < 0)
-- return -1;
--
-- mesh_flags &= MESH_FLAGS_AE;
--
- switch (hdr->frame_control &
- cpu_to_le16(IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) {
- case cpu_to_le16(IEEE80211_FCTL_TODS):
-@@ -608,17 +644,6 @@ int ieee80211_data_to_8023_exthdr(struct
- iftype != NL80211_IFTYPE_AP_VLAN &&
- iftype != NL80211_IFTYPE_STATION))
- return -1;
-- if (iftype == NL80211_IFTYPE_MESH_POINT) {
-- if (mesh_flags == MESH_FLAGS_AE_A4)
-- return -1;
-- if (mesh_flags == MESH_FLAGS_AE_A5_A6 &&
-- skb_copy_bits(skb, hdrlen +
-- offsetof(struct ieee80211s_hdr, eaddr1),
-- tmp.h_dest, 2 * ETH_ALEN) < 0)
-- return -1;
--
-- hdrlen += __ieee80211_get_mesh_hdrlen(mesh_flags);
-- }
- break;
- case cpu_to_le16(IEEE80211_FCTL_FROMDS):
- if ((iftype != NL80211_IFTYPE_STATION &&
-@@ -627,16 +652,6 @@ int ieee80211_data_to_8023_exthdr(struct
- (is_multicast_ether_addr(tmp.h_dest) &&
- ether_addr_equal(tmp.h_source, addr)))
- return -1;
-- if (iftype == NL80211_IFTYPE_MESH_POINT) {
-- if (mesh_flags == MESH_FLAGS_AE_A5_A6)
-- return -1;
-- if (mesh_flags == MESH_FLAGS_AE_A4 &&
-- skb_copy_bits(skb, hdrlen +
-- offsetof(struct ieee80211s_hdr, eaddr1),
-- tmp.h_source, ETH_ALEN) < 0)
-- return -1;
-- hdrlen += __ieee80211_get_mesh_hdrlen(mesh_flags);
-- }
- break;
- case cpu_to_le16(0):
- if (iftype != NL80211_IFTYPE_ADHOC &&
-@@ -646,7 +661,7 @@ int ieee80211_data_to_8023_exthdr(struct
- break;
- }
-
-- if (likely(!is_amsdu &&
-+ if (likely(!is_amsdu && iftype != NL80211_IFTYPE_MESH_POINT &&
- skb_copy_bits(skb, hdrlen, &payload, sizeof(payload)) == 0 &&
- ieee80211_get_8023_tunnel_proto(&payload, &tmp.h_proto))) {
- /* remove RFC1042 or Bridge-Tunnel encapsulation */
-@@ -722,7 +737,8 @@ __ieee80211_amsdu_copy_frag(struct sk_bu
-
- static struct sk_buff *
- __ieee80211_amsdu_copy(struct sk_buff *skb, unsigned int hlen,
-- int offset, int len, bool reuse_frag)
-+ int offset, int len, bool reuse_frag,
-+ int min_len)
- {
- struct sk_buff *frame;
- int cur_len = len;
-@@ -736,7 +752,7 @@ __ieee80211_amsdu_copy(struct sk_buff *s
- * in the stack later.
- */
- if (reuse_frag)
-- cur_len = min_t(int, len, 32);
-+ cur_len = min_t(int, len, min_len);
-
- /*
- * Allocate and reserve two bytes more for payload
-@@ -746,6 +762,7 @@ __ieee80211_amsdu_copy(struct sk_buff *s
- if (!frame)
- return NULL;
-
-+ frame->priority = skb->priority;
- skb_reserve(frame, hlen + sizeof(struct ethhdr) + 2);
- skb_copy_bits(skb, offset, skb_put(frame, cur_len), cur_len);
-
-@@ -762,23 +779,37 @@ __ieee80211_amsdu_copy(struct sk_buff *s
- void ieee80211_amsdu_to_8023s(struct sk_buff *skb, struct sk_buff_head *list,
- const u8 *addr, enum nl80211_iftype iftype,
- const unsigned int extra_headroom,
-- const u8 *check_da, const u8 *check_sa)
-+ const u8 *check_da, const u8 *check_sa,
-+ bool mesh_control)
- {
- unsigned int hlen = ALIGN(extra_headroom, 4);
- struct sk_buff *frame = NULL;
- int offset = 0, remaining;
-- struct ethhdr eth;
-+ struct {
-+ struct ethhdr eth;
-+ uint8_t flags;
-+ } hdr;
- bool reuse_frag = skb->head_frag && !skb_has_frag_list(skb);
- bool reuse_skb = false;
- bool last = false;
-+ int copy_len = sizeof(hdr.eth);
-+
-+ if (iftype == NL80211_IFTYPE_MESH_POINT)
-+ copy_len = sizeof(hdr);
-
- while (!last) {
- unsigned int subframe_len;
-- int len;
-+ int len, mesh_len = 0;
- u8 padding;
-
-- skb_copy_bits(skb, offset, ð, sizeof(eth));
-- len = ntohs(eth.h_proto);
-+ skb_copy_bits(skb, offset, &hdr, copy_len);
-+ if (iftype == NL80211_IFTYPE_MESH_POINT)
-+ mesh_len = __ieee80211_get_mesh_hdrlen(hdr.flags);
-+ if (mesh_control)
-+ len = le16_to_cpu(*(__le16 *)&hdr.eth.h_proto) + mesh_len;
-+ else
-+ len = ntohs(hdr.eth.h_proto);
-+
- subframe_len = sizeof(struct ethhdr) + len;
- padding = (4 - subframe_len) & 0x3;
-
-@@ -787,16 +818,16 @@ void ieee80211_amsdu_to_8023s(struct sk_
- if (subframe_len > remaining)
- goto purge;
- /* mitigate A-MSDU aggregation injection attacks */
-- if (ether_addr_equal(eth.h_dest, rfc1042_header))
-+ if (ether_addr_equal(hdr.eth.h_dest, rfc1042_header))
- goto purge;
-
- offset += sizeof(struct ethhdr);
- last = remaining <= subframe_len + padding;
-
- /* FIXME: should we really accept multicast DA? */
-- if ((check_da && !is_multicast_ether_addr(eth.h_dest) &&
-- !ether_addr_equal(check_da, eth.h_dest)) ||
-- (check_sa && !ether_addr_equal(check_sa, eth.h_source))) {
-+ if ((check_da && !is_multicast_ether_addr(hdr.eth.h_dest) &&
-+ !ether_addr_equal(check_da, hdr.eth.h_dest)) ||
-+ (check_sa && !ether_addr_equal(check_sa, hdr.eth.h_source))) {
- offset += len + padding;
- continue;
- }
-@@ -808,7 +839,7 @@ void ieee80211_amsdu_to_8023s(struct sk_
- reuse_skb = true;
- } else {
- frame = __ieee80211_amsdu_copy(skb, hlen, offset, len,
-- reuse_frag);
-+ reuse_frag, 32 + mesh_len);
- if (!frame)
- goto purge;
-
-@@ -819,10 +850,11 @@ void ieee80211_amsdu_to_8023s(struct sk_
- frame->dev = skb->dev;
- frame->priority = skb->priority;
-
-- if (likely(ieee80211_get_8023_tunnel_proto(frame->data, ð.h_proto)))
-+ if (likely(iftype != NL80211_IFTYPE_MESH_POINT &&
-+ ieee80211_get_8023_tunnel_proto(frame->data, &hdr.eth.h_proto)))
- skb_pull(frame, ETH_ALEN + 2);
-
-- memcpy(skb_push(frame, sizeof(eth)), ð, sizeof(eth));
-+ memcpy(skb_push(frame, sizeof(hdr.eth)), &hdr.eth, sizeof(hdr.eth));
- __skb_queue_tail(list, frame);
- }
-
projects / openwrt / openwrt.git / blobdiff